#include <sr09.h>

Inheritance diagram for SR09:

Public Member Functions
virtual void	construct (EtherCAT_SlaveHandler *sh, int &start_address)
	Construct function, run at startup to set SyncManagers and FMMUs. More...

virtual int	initialize (hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)

virtual void	multiDiagnostics (vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
	This function gives some diagnostics data. More...

virtual void	packCommand (unsigned char *buffer, bool halt, bool reset)
	packs the commands before sending them to the EtherCAT bus More...

	SR09 ()
	Constructor of the SR09 driver. More...

virtual bool	unpackState (unsigned char this_buffer, unsigned char prev_buffer)
	This functions receives data from the EtherCAT bus. More...

Public Member Functions inherited from SrEdc
void	build_CAN_message (ETHERCAT_CAN_BRIDGE_DATA *message)

bool	can_data_is_ack (ETHERCAT_CAN_BRIDGE_DATA *packet)
	This function checks if the can packet in the unpackState() this_buffer is an ACK. More...

virtual void	construct (EtherCAT_SlaveHandler *sh, int &start_address, unsigned int ethercat_command_data_size, unsigned int ethercat_status_data_size, unsigned int ethercat_can_bridge_data_size, unsigned int ethercat_command_data_address, unsigned int ethercat_status_data_address, unsigned int ethercat_can_bridge_data_command_address, unsigned int ethercat_can_bridge_data_status_address)
	Construct function, run at startup to set SyncManagers and FMMUs. More...

void	erase_flash ()
	Erase the PIC18F Flash memory. More...

bool	read_flash (unsigned int offset, unsigned int baddr)
	Function that reads back 8 bytes from PIC18F program memory. More...

bool	simple_motor_flasher (sr_robot_msgs::SimpleMotorFlasher::Request &req, sr_robot_msgs::SimpleMotorFlasher::Response &res)
	ROS Service that flashes a new firmware into a SimpleMotor board. More...

	SrEdc ()
	Constructor of the SrEdc driver. More...

Protected Types
typedef realtime_tools::RealtimePublisher< std_msgs::Int16 >	rt_pub_int16_t

Protected Types inherited from SrEdc
typedef realtime_tools::RealtimePublisher< std_msgs::Int16 >	rt_pub_int16_t

Protected Types inherited from SR0X
enum	{ MODE_OFF = 0x00, MODE_ENABLE = (1 << 0), MODE_CURRENT = (1 << 1), MODE_SAFETY_RESET = (1 << 4), MODE_SAFETY_LOCKOUT = (1 << 5), MODE_UNDERVOLTAGE = (1 << 6), MODE_RESET = (1 << 7) }

enum	{ EC_PRODUCT_ID_BRIDGE = 0, EC_PRODUCT_ID_SHADOWCAN = 2, EC_PRODUCT_ID_DUALMOTOR = 3 }

Protected Member Functions
virtual void	get_board_id_and_can_bus (int board_id, int can_bus, unsigned int board_can_id)

bool	imu_scale_callback_ (sr_robot_msgs::SetImuScale::Request &request, sr_robot_msgs::SetImuScale::Response &response, const char *which)

virtual void	reinitialize_boards ()
	This function will call the reinitialization function for the boards attached to the CAN bus. More...

Protected Attributes
boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > >	extra_analog_inputs_publisher
	Extra analog inputs real time publisher (+ accelerometer and gyroscope) More...

std::vector< boost::shared_ptr< rt_pub_int16_t > >	realtime_pub_

Protected Attributes inherited from SrEdc
bool	can_packet_acked

int	counter_

std::string	device_id_

std::string	device_joint_prefix_

boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > >	extra_analog_inputs_publisher
	Extra analog inputs real time publisher (+ accelerometer and gyroscope) More...

bool	flashing

ros::NodeHandle	nh_tilde_

ros::NodeHandle	nodehandle_

std::vector< boost::shared_ptr< rt_pub_int16_t > >	realtime_pub_

Protected Attributes inherited from SR0X
uint8_t	board_major_

uint8_t	board_minor_

int	command_base_

int	device_offset_

uint8_t	fw_major_

uint8_t	fw_minor_

int	level_

string	reason_

int	status_base_

Private Member Functions
void	readImu (ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *status)
	This funcion reads the ethercat status and fills the imu_state with the relevant values. More...

Private Attributes
int16_t	cycle_count

boost::shared_ptr< realtime_tools::RealtimePublisher< sr_robot_msgs::EthercatDebug > >	debug_publisher
	Debug real time publisher: publishes the raw ethercat data. More...

ros_ethercat_model::RobotState *	hw_

ros::ServiceServer	imu_acc_scale_server_

ros::ServiceServer	imu_gyr_scale_server_

int	imu_scale_acc_

bool	imu_scale_change_

int	imu_scale_gyr_

ros_ethercat_model::ImuState *	imu_state_

boost::shared_ptr< shadow_robot::SrMotorHandLib< ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND > >	sr_hand_lib

unsigned int	zero_buffer_read

Detailed Description

Definition at line 57 of file sr09.h.

Member Typedef Documentation

◆ rt_pub_int16_t

typedef realtime_tools::RealtimePublisher<std_msgs::Int16> SR09::rt_pub_int16_t

protected

Definition at line 74 of file sr09.h.

Constructor & Destructor Documentation

◆ SR09()

SR09::SR09 ( )

Constructor of the SR09 driver.

This is the Constructor of the driver. We initialize a few boolean values, a mutex and create the Bootloading service.

Definition at line 78 of file sr09.cpp.

Member Function Documentation

◆ construct()

void SR09::construct	(	EtherCAT_SlaveHandler *	sh,
		int &	start_address
	)

virtual

Construct function, run at startup to set SyncManagers and FMMUs.

The role of this function is to setup the SyncManagers and the FMMUs used by this EtherCAT slave. This slave is using two Mailboxes on two different memory areas.

Here we are setting up the way of communicating between ROS and the PIC32 using the EtherCAT protocol.

We communicate using Shared Memory areas.

The FMMUs are usefull to map the logical memory used by ROS to the Physical memory of the EtherCAT slave chip (ET1200 chip). So that the chip receiving the packet will know that the data at address 0x10000 is in reality to be written at physical address 0x1000 of the chip memory for example. It is the mapping between the EtherCAT bus address space and each slave's chip own memory address space.

The SyncManagers are usefull to give a safe way of accessing this Shared Memory, using a consumer / producer model. There are features like interrupts to tell the consumer that there is something to consume or to tell the producer that the Mailbox is empty and then ready to receive a new message.

One Mailbox contains the commands, written by ROS, read by the PIC32
One Mailbox contains the status, written back by the PIC32, read by ROS

That's basically one Mailbox for upstream and one Mailbox for downstream.

The first Mailbox contains in fact two commands, one is the torque demand, the other is a CAN command used in CAN_DIRECT_MODE to communicate with the SimpleMotor for test purposes, or to reflash a new firmware in bootloading mode. This Mailbox is at logicial address 0x10000 and mapped via a FMMU to physical address 0x1000 (the first address of user memory)
The second Mailbox contains in fact two status, they are the response of the two previously described commands. One is the status containing the joints data, sensor data, finger tips data and motor data. The other is the can command response in CAN_DIRECT_MODE. When doing a flashing in bootloading mode this is usually an acknowledgment from the bootloader. This Mailbox is at logical address 0x10038 and mapped via a FMMU to physical address 0x1038.

This function sets the two private members command_size_ and status_size_ to be the size of each Mailbox. It is important for these numbers to be accurate since they are used by the EthercatHardware class when manipulating the buffers. If you need to have several commands like in this SR09 driver, put the sum of the size, same thing for the status.

Definition at line 126 of file sr09.cpp.

◆ get_board_id_and_can_bus()

void SR09::get_board_id_and_can_bus	(	int	board_id,
		int *	can_bus,
		unsigned int *	board_can_id
	)

protectedvirtual

Given the identifier for a certain board (motor board/ muscle driver) determines the right value for the CAN bus and the ID of the board in that CAN bus.

Parameters

board_id	the unique identifier for the board
can_bus	pointer to the can bus number we want to determine
board_can_id	pointer to the board id we want to determine

Implements SrEdc.

Definition at line 526 of file sr09.cpp.

◆ imu_scale_callback_()

bool SR09::imu_scale_callback_	(	sr_robot_msgs::SetImuScale::Request &	request,
		sr_robot_msgs::SetImuScale::Response &	response,
		const char *	which
	)

protected

Definition at line 187 of file sr09.cpp.

◆ initialize()

int SR09::initialize	(	hardware_interface::HardwareInterface *	hw,
		bool	allow_unprogrammed = `true`
	)

virtual

Reimplemented from SR0X.

Definition at line 139 of file sr09.cpp.

◆ multiDiagnostics()

void SR09::multiDiagnostics	(	vector< diagnostic_msgs::DiagnosticStatus > &	vec,
		unsigned char *	buffer
	)

virtual

This function gives some diagnostics data.

This function provides diagnostics data that can be displayed by the runtime_monitor node. We use the mutliDiagnostics as it publishes the diagnostics for each motors.

Definition at line 219 of file sr09.cpp.

◆ packCommand()

void SR09::packCommand	(	unsigned char *	buffer,
		bool	halt,
		bool	reset
	)

virtual

packs the commands before sending them to the EtherCAT bus

This is one of the most important functions of this driver. This function is called each millisecond (1 kHz freq) by the EthercatHardware::update() function in the controlLoop() of the ros_etherCAT node.

This function is called with a buffer as a parameter, the buffer provided is where we write the commands to send via EtherCAT.

We just cast the buffer to our structure type, fill the structure with our data, then add the structure size to the buffer address to shift into memory and access the second command. The buffer has been allocated with command_size_ bytes, which is the sum of the two command size, so we have to put the two commands one next to the other. In fact we access the buffer using this kind of code :

ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND *command = (ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND *)buffer;

ETHERCAT_CAN_BRIDGE_DATA *message = (ETHERCAT_CAN_BRIDGE_DATA *)(buffer + ETHERCAT_COMMAND_DATA_SIZE);

Definition at line 271 of file sr09.cpp.

◆ readImu()

void SR09::readImu ( ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS * status )

private

This funcion reads the ethercat status and fills the imu_state with the relevant values.

Definition at line 334 of file sr09.cpp.

◆ reinitialize_boards()

void SR09::reinitialize_boards ( )

protectedvirtual

This function will call the reinitialization function for the boards attached to the CAN bus.

Implements SrEdc.

Definition at line 520 of file sr09.cpp.

◆ unpackState()

bool SR09::unpackState	(	unsigned char *	this_buffer,
		unsigned char *	prev_buffer
	)

virtual

This functions receives data from the EtherCAT bus.

This function allows the driver to get the data present on the EtherCAT bus and intended for us.

It gives us access to the logical memory registered during the construct().

In order to be able to do differentials two buffers are kept, this_buffer is the actual data that has just been received and prev_buffer is the previous buffer received from the EtherCAT bus.

We access the data sent by PIC32 here using the same tricks we used in packCommand().

ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *tbuffer = (ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *)(this_buffer + command_size_);

ETHERCAT_CAN_BRIDGE_DATA *can_data = (ETHERCAT_CAN_BRIDGE_DATA *)(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE);

Parameters

this_buffer	The data just being received by EtherCAT
prev_buffer	The previous data received by EtherCAT

Definition at line 393 of file sr09.cpp.

Member Data Documentation

◆ cycle_count

int16_t SR09::cycle_count

private

a counter used to publish the tactiles at 100Hz: count 10 cycles, then reset the cycle_count to 0.

Definition at line 120 of file sr09.h.

◆ debug_publisher

boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> > SR09::debug_publisher

private

Debug real time publisher: publishes the raw ethercat data.

Definition at line 125 of file sr09.h.

◆ extra_analog_inputs_publisher

boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > SR09::extra_analog_inputs_publisher

protected

Extra analog inputs real time publisher (+ accelerometer and gyroscope)

Definition at line 78 of file sr09.h.

◆ hw_

ros_ethercat_model::RobotState* SR09::hw_

private

Definition at line 107 of file sr09.h.

◆ imu_acc_scale_server_

ros::ServiceServer SR09::imu_acc_scale_server_

private

Definition at line 101 of file sr09.h.

◆ imu_gyr_scale_server_

ros::ServiceServer SR09::imu_gyr_scale_server_

private

Definition at line 100 of file sr09.h.

◆ imu_scale_acc_

int SR09::imu_scale_acc_

private

Definition at line 114 of file sr09.h.

◆ imu_scale_change_

bool SR09::imu_scale_change_

private

Definition at line 115 of file sr09.h.

◆ imu_scale_gyr_

int SR09::imu_scale_gyr_

private

Definition at line 113 of file sr09.h.

◆ imu_state_

ros_ethercat_model::ImuState* SR09::imu_state_

private

Definition at line 109 of file sr09.h.

◆ realtime_pub_

std::vector<boost::shared_ptr<rt_pub_int16_t> > SR09::realtime_pub_

protected

Definition at line 75 of file sr09.h.

◆ sr_hand_lib

boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND> > SR09::sr_hand_lib

private

Definition at line 111 of file sr09.h.

◆ zero_buffer_read

unsigned int SR09::zero_buffer_read

private

Definition at line 104 of file sr09.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ rt_pub_int16_t

Constructor & Destructor Documentation

◆ SR09()

Member Function Documentation

◆ construct()

◆ get_board_id_and_can_bus()

◆ imu_scale_callback_()

◆ initialize()

◆ multiDiagnostics()

◆ packCommand()

◆ readImu()

◆ reinitialize_boards()

◆ unpackState()

Member Data Documentation

◆ cycle_count

◆ debug_publisher

◆ extra_analog_inputs_publisher

◆ hw_

◆ imu_acc_scale_server_

◆ imu_gyr_scale_server_

◆ imu_scale_acc_

◆ imu_scale_change_

◆ imu_scale_gyr_

◆ imu_state_

◆ realtime_pub_

◆ sr_hand_lib

◆ zero_buffer_read