A package implementing a ROS interface for the etherCAT Shadow Robot Dextrous Hand.
sr_edc_ethercat_drivers is the low level communication library that allows you to communicate with the SR EDC EtherCAT hand.
This package takes care of the low level EtherCAT protocol, it sends data to the PIC32 which is on the palm, and receives back informations from it.
It implements the driver for the Ethercat slave device which is on the SR EDC EtherCAT hand.
It is implemented as plugins.
When the ros_etherCAT node runs, it discovers the EtherCAT slaves which are present on the EtherCAT bus, and loads dynamically the class needed to control each slave.
This package implements those classes, to control the EtherCAT slaves on the SR EDC EtherCAT hand, and links them into a library so that ros_etherCAT can load driver classes from it.
This document will describe how the EtherCAT driver works, but first we have to understand how the ros_etherCAT node works.
root@rosethercat:~# roslaunch sr_edc_launch sr_edc.launch
rosservice call SimpleMotorFlasher "/home/hand/test-firmware.hex" 8
This means we call the "SimpleMotorFlasher" service, and tell it to flash the "test-firmware.hex" firmware to the motor board number 8.
At the moment the flashing procedure has been tested with 2 firmwares :
BEWARE : the firmware you flash has to be "bootloader aware" in order to allow future re-flashing. Upon reception of a Magic Packet it has to :
If the firmware you flash does not support the Magic Packet, then you won't be able to bootload anymore, untill you overwrite yourself the last location in EEPROM with 0xFF and restart the motor pcb.
The current SimpleMotor firmware is bootloader aware and therefore can be flashed over itself or overwritten by something else without any problem.
There is basically three things to do in order to do a bootloader-aware firmware :
Here is the linkerscript used for the SimpleMotor and led-blinker firmwares :
#DEFINE _CODEEND _DEBUGCODESTART - 1 #DEFINE _CEND _CODEEND + _DEBUGCODELEN #DEFINE _DATAEND _DEBUGDATASTART - 1 #DEFINE _DEND _DATAEND + _DEBUGDATALEN LIBPATH . FILES clib.lib // do not need anymore, its included in the project //FILES c018i-userapp.o FILES p18f2580.lib CODEPAGE NAME=startup START=0x0 END=0x5 PROTECTED CODEPAGE NAME=boot START=0x8 END=0x47F PROTECTED CODEPAGE NAME=vectors START=0x480 END=0x49F PROTECTED #IFDEF _DEBUGCODESTART CODEPAGE NAME=page START=0x4c0 END=_CODEEND CODEPAGE NAME=debug START=_DEBUGCODESTART END=_CEND PROTECTED #ELSE CODEPAGE NAME=page START=0x4c0 END=0x7FFF #FI CODEPAGE NAME=idlocs START=0x200000 END=0x200007 PROTECTED CODEPAGE NAME=config START=0x300000 END=0x30000D PROTECTED CODEPAGE NAME=devid START=0x3FFFFE END=0x3FFFFF PROTECTED CODEPAGE NAME=eedata START=0xF00000 END=0xF000FF PROTECTED ACCESSBANK NAME=accessram START=0x0 END=0x5F DATABANK NAME=gpr0 START=0x80 END=0xFF DATABANK NAME=gpr1 START=0x100 END=0x1FF DATABANK NAME=gpr2 START=0x200 END=0x2FF DATABANK NAME=gpr3 START=0x300 END=0x3FF DATABANK NAME=gpr4 START=0x400 END=0x4FF #IFDEF _DEBUGDATASTART DATABANK NAME=gpr5 START=0x500 END=_DATAEND DATABANK NAME=dbgspr START=_DEBUGDATASTART END=_DEND PROTECTED #ELSE //no debug DATABANK NAME=gpr5 START=0x500 END=0x5FF #FI ACCESSBANK NAME=accesssfr START=0xF60 END=0xFFF PROTECTED DATABANK NAME=sfr13 START=0xD00 END=0xDFF PROTECTED DATABANK NAME=sfr14 START=0xE00 END=0xEFF PROTECTED DATABANK NAME=sfr15 START=0xF00 END=0xF5F PROTECTED #IFDEF _CRUNTIME SECTION NAME=CONFIG ROM=config #IFDEF _DEBUGDATASTART STACK SIZE=0x100 RAM=gpr4 #ELSE STACK SIZE=0x100 RAM=gpr5 #FI #FI
Note : this file is located at https://github.com/shadow-robot/hand-firmware/tree/master/EDC/simplemotor/simplemotor.lkr
Here is the startup code you will need to put in your project :
/* $Id: c018i.c,v 1.7 2006/11/15 22:53:12 moshtaa Exp $ */ /* Copyright (c)1999 Microchip Technology */ /* Modified by Yann Sionneau in 2011 */ /* MPLAB-C18 startup code, including initialized data */ /* external reference to __init() function */ extern void __init(void); /* external reference to the user's main routine */ extern void main(void); /* prototype for the startup function */ void _entry(void); void _startup(void); /* prototype for the initialized data setup */ void _do_cinit(void); extern volatile near unsigned long short TBLPTR; extern near unsigned FSR0; extern near char __FPFLAGS; #define RND 6 #pragma code _entry_scn=0x0004c0 void _entry(void) { _asm goto _startup _endasm } #pragma code _startup_scn void _startup(void) { _asm // Initialize the stack pointer lfsr 1, _stack lfsr 2, _stack clrf TBLPTRU, 0 // 1st silicon doesn't do this on POR bcf __FPFLAGS, RND, 0 // Initialize rounding flag for floating point libs _endasm _do_cinit(); loop: // If user defined __init is not found, the one in clib.lib will be used __init(); // Call the user's main routine main(); goto loop; } /* end _startup() */ /* MPLAB-C18 initialized data memory support */ /* The linker will populate the _cinit table */ extern far rom struct { unsigned short num_init; struct _init_entry { unsigned long from; unsigned long to; unsigned long size; } entries[]; } _cinit; #pragma code _cinit_scn void _do_cinit(void) { /* we'll make the assumption in the following code that these statics will be allocated into the same bank. */ static short long prom; static unsigned short curr_byte; static unsigned short curr_entry; static short long data_ptr; // Initialized data... TBLPTR = (short long) &_cinit; _asm movlb data_ptr tblrdpostinc movf TABLAT, 0, 0 movwf curr_entry, 1 tblrdpostinc movf TABLAT, 0, 0 movwf curr_entry + 1, 1 _endasm //while (curr_entry) //{ test: _asm bnz 3 tstfsz curr_entry, 1 bra 1 _endasm goto done; /* Count down so we only have to look up the data in _cinit once. At this point we know that TBLPTR points to the top of the current entry in _cinit, so we can just start reading the from, to, and size values. */ _asm /* read the source address */ tblrdpostinc movf TABLAT, 0, 0 movwf prom, 1 tblrdpostinc movf TABLAT, 0, 0 movwf prom + 1, 1 tblrdpostinc movf TABLAT, 0, 0 movwf prom + 2, 1 /* skip a byte since it's stored as a 32bit int */ tblrdpostinc /* read the destination address directly into FSR0 */ tblrdpostinc movf TABLAT, 0, 0 movwf FSR0L, 0 tblrdpostinc movf TABLAT, 0, 0 movwf FSR0H, 0 /* skip two bytes since it's stored as a 32bit int */ tblrdpostinc tblrdpostinc /* read the destination address directly into FSR0 */ tblrdpostinc movf TABLAT, 0, 0 movwf curr_byte, 1 tblrdpostinc movf TABLAT, 0, 0 movwf curr_byte + 1, 1 /* skip two bytes since it's stored as a 32bit int */ tblrdpostinc tblrdpostinc _endasm //prom = data_ptr->from; //FSR0 = data_ptr->to; //curr_byte = (unsigned short) data_ptr->size; /* the table pointer now points to the next entry. Save it off since we'll be using the table pointer to do the copying for the entry. */ data_ptr = TBLPTR; /* now assign the source address to the table pointer */ TBLPTR = prom; /* do the copy loop */ _asm // determine if we have any more bytes to copy movlb curr_byte movf curr_byte, 1, 1 copy_loop: bnz 2 // copy_one_byte movf curr_byte + 1, 1, 1 bz 7 // done_copying copy_one_byte: tblrdpostinc movf TABLAT, 0, 0 movwf POSTINC0, 0 // decrement byte counter decf curr_byte, 1, 1 bc - 8 // copy_loop decf curr_byte + 1, 1, 1 bra - 7 // copy_one_byte done_copying: _endasm /* restore the table pointer for the next entry */ TBLPTR = data_ptr; /* next entry... */ curr_entry--; goto test; done:; } \ endcode Note : This code is located at https://github.com/shadow-robot/hand-firmware/tree/master/EDC/simplemotor/c018i-userapp.c \ section basic basic workflow of the SR06 ROS EtherCAT driver \ image html pr2_etherCAT. svg The ros_etherCAT node has a controlLoop() which is an infinite loop that runs EthercatHardware::update() EthercatHardware::update() will do two things : - Loop over EtherCAT slaves and run their SR06::packCommand() function. - Loop again over EtherCAT slaves andrun their SR06::unpackState() function. The loop frequency is 1 kHz. Basically, the node sends a command to each slave, andthen receive the state of each slave, in a loop . \ section packcommand SR06::packCommand() This function is called once per millisecond by the controlLoop of ros_etherCAT node on each etherCAT slave. SR06::packCommand() is the function that fills the buffers to be sent via EtherCAT to the SR EDC EtherCAT hand . Several commands can be packed, the sr06 driver for example is sending two commands. See the SR06::packCommand() for more information about this function. In this case the command_size_ attribute should be the sum of the size of the two commands . \ section bootloadingprotocol Bootloading protocole The bootloading is done through CAN, so the bootloader(which is on the PIC18F simplemotor board) waits for CAN commands in order to execute them to erase flash, write flash andso on. But the bootloading is asked andmanaged by ROS( using the ROS service call), ROS in theory does not send any CAN message, ROS only interacts with the PIC32 through the EtherCAT bus. So in order to do the bootloading, ROS sets the first member of the first structure of the "command" EtherCAT Mailbox(which is named EDC_command) to be : EDC_COMMAND_CAN_TEST_MODE. Doing this will put the PIC32 in "CAN_TEST_MODE". So that afterward ROS can put can messages in the second structure of the "command" EtherCAT Mailbox, the PIC32 will read these can messages and send them over the CAN bus . Moreover any CAN message received by the PIC32 will be forwarded back to ROS through the EtherCAT "status" Mailbox. So that the ROS flashing service can check for ACK from the PIC18F during the bootloading process . Eventually ROS is able to send CAN message, putting the PIC32 is "CAN_TEST_MODE" and then execute the bootloading sequence on a SimpleMotor PCB. The bootloading protocol is described in the following subsection. \ subsection SIDbitmapping CAN SID bit mapping A CAN SID is 11 bits-wide. - The first two bits are for the direction of the message - 10 : Incoming (for the PIC18F ) - 01 : Outgoing (for the PIC18F ) - 11 : bootloader protocol related CAN message (This is the one used for bootloading) - 00 : reserved - The following 4 bits are the motor_id(0 to 9) - The following bit is an ACK bit - 0 : Packet is a request - 1 : Packet is an ACK - The following 4 bits are - The Request_Data_Type, if the direction bits are "10" - The Command ID, if the direction bits are "11" ( bootloader mode ) \ subsection canbootloadercommands CAN bootloader commands - "0000" (0x0) : WRITE_FLASH_DATA_COMMAND - "0001" (0x1) : READ_FLASH_COMMAND - "0010" (0x2) : ERASE_FLASH_COMMAND - "0011" (0x3) : RESET_COMMAND - "0100" (0x4) : READ_VERSION_COMMAND - "0101" (0x5) : WRITE_FLASH_ADDRESS_COMMAND - "1010" (0xA) : MAGIC_PACKET \ subsection canbootloadercommandwriteflashdatacommand WRITE_FLASH_DATA_COMMAND This command : - Has a length of 8 bytes - Has a SID like this : 0x600 | (motor_id << 5) | 0 - Contains 8 bytes of data to be written into program memory (flash memory) - Gets answered with the usual acknowledgement CAN message(same SID but with 0x10 bit on, same length, same data ) It triggers this code in the PIC18F bootloader : \ code /* This function does the actual writting in program memory (which is flash) Writting the flash has to be done by 32 bytes blocks. But we can only transport 8 bytes of data in a CAN message, so we do the block writting in 4 CAN commands. So this command writes 8 bytes, and is called several times (4 times in theory) Each time it is called the "position" variable gets added 8. When position gets equal to 32, it means we have buffered a block and we can start the writting procedure. */ static void write_flash_data(void) { int8 i; for (i = 0; i < 8; ++i) // Put 8 more bytes in the buffer { TABLAT = CanMsgR.d.byte[i]; _asm TBLWTPOSTINC _endasm } position += 8; // We just buffered 8 more bytes if (position == 32) { // We have buffured a 32 bytes block, we can start the writting procedure _asm TBLRDPOSTDEC _endasm // We do a TBLRDPOSTDEC in order for the TBLPTR addressing register to stay in the range of the 32 bytes // block we are writting, this is necessary because there has been one extra unneeded TBLWTPOSTINC during // the previous loop. If we don't do that, the block will be written over the next block, so 32 bytes after // the address provided by the previous WRITE_FLASH_ADDRESS command. EECON1 |= ((1 << EEPGD) | (1 << WREN)); // point to Flash program memory & enable write to memory EECON1 &= ~(1 << CFGS); // access Flash program memory INTCON &= ~(1 << GIE); // disable interrupts EECON2 = 0x55; //magic enable EECON2 = 0xaa; EECON1 |= (1 << WR); // starts the actual writting (CPU stall) INTCON |= (1 << GIE); // re-enable interrupts EECON1 &= ~(1 << WREN); // disable write to memory position = 0; // reset the position to 0, we just finished a 32 bytes block } } \ endcode \ subsection canbootloadercommandreadflashcommand READ_FLASH_COMMAND This command : - Has a length of 3 bytes (which are the address where to read ) - Has a SID like this : 0x600 | (motod_id << 5) | 0x1 - Gets answered with a CAN message of 8 bytes long, containing 8 bytes of data located at the address received with the command It triggers this code in the PIC18F bootloader : \ code /* This function reads the program memory (which is flash) It reads 8 bytes at the address present in the incoming CAN command and sends it back through CAN too. */ static void read_flash(void) { overlay int8 i = 0; TBLPTRL = CanMsgR.d.byte[0]; TBLPTRH = CanMsgR.d.byte[1]; TBLPTRU = CanMsgR.d.byte[2]; while (i < 8) { _asm TBLRDPOSTINC _endasm CanMsgT.d.byte[i++] = TABLAT; } CanMsgT.messageID = CanMsgR.messageID | 0x10; // We set the "ACK" bit in the SID CanMsgT.length = 8; sendCanMsg(); // We send it } \ endcode \ subsection canbootloadercommanderaseflashcommand ERASE_FLASH_COMMAND This command : - Has a length of 0 byte. - Has a SID like this : 0x600 | (motod_id << 5) | 0x2 - Gets answered with the usual acknowledgement CAN message(same SID but with 0x10 bit on, same length, same data ) It triggers this code in the PIC18F bootloader : \ code /* This function erases the program memory (which is flash) It only erases the user application So it starts erasing after the bootloader and stops before the debugger code */ static void erase_flash(void) { overlay int16 i = 0; i = 0x04c0; // user application start address // 0x7dc0 is the start of debugger code while (i < 0x7dc0) // and we don't wanna erase debugger code { TBLPTR = i; TBLPTR &= 0xFFFFE0; EECON1 |= 128; // point to Flash program memory EECON1 &= ~64; // access Flash program memory EECON1 |= 4; // enable write to memory EECON1 |= 16; // enable Row Erase operation INTCON &= ~128; // disable interrupts EECON2 = 0x55; EECON2 = 0xaa; EECON1 |= 2; // start erase (CPU stall) while (EECON1 & 2) { } // Wait for stuff being written INTCON |= 128; i += 64; } } \ endcode \ subsection canbootloadercommandresetcommand RESET_COMMAND This command : - Has a length of 0 byte. - Has a SID like this : 0x600 | (motod_id << 5) | 0x3 - Gets answered with the usual acknowledgement CAN message(same SID but with 0x10 bit on, same length, same data ) It triggers this code in the PIC18F bootloader : \ code write_eeprom(); acknowledge_packet(); _asm goto 0x0000 // This jumps to the Reset vector _endasm \ endcode With write_eeprom() being : \ code /* This function writes to the EEPROM memory The only goal of this function is to write something different than 0xFF at the last memory location in EEPROM. This happens right after a flashing operation has been successfull, to make sure that at the following reboot of the PIC18F, it will boot directly to the user application and not stay in bootloader mode. This function writes 0x42 to the last byte (address 0xFF). */ static void write_eeprom(void) { EEADR = 0xff; // EEPROM address to write to EEDATA = 0x42; // Something != than 0xFF it's the data to be written EECON1 &= ~((1 << EEPGD) | (1 << CFGS)); // we select EEPROM memory EECON1 |= (1 << WREN); // enable write to memory INTCON &= ~(1 << GIE); // disable interrupts EECON2 = 0x55; // magic enable EECON2 = 0xaa; EECON1 |= (1 << WR); // do the write while (EECON1 & (1 << WR)) { } // wait for the write to finish INTCON |= (1 << GIE); // enable back interrupts EECON1 &= ~(1 << WREN); // disable write to memory } \ endcode \ subsection canbootloadercommandreadversioncommand READ_VERSION_COMMAND This command : - Has a length of 0 byte. - Has a SID like this : 0x600 | (motod_id << 5) | 0x4 - Gets answered with a 4 bytes long CAN message, with the same SID(with the 0x10 ACK bit on), the data containing the major andminor version number of the bootloader. (for the moment it always returns 0x40414243) It triggers this code in the PIC18F bootloader : \ code CanMsgT . messageID = CanMsgR.messageID | 0x10; CanMsgT.d.byte[0] = 0x40; CanMsgT.d.byte[1] = 0x41; CanMsgT.d.byte[2] = 0x42; CanMsgT.d.byte[3] = 0x43; CanMsgT. length = 4; sendCanMsg(); \ endcode \ subsection canbootloadercommandwriteflashaddresscommand WRITE_FLASH_ADDRESS_COMMAND This command : - Has a length of 3 bytes. - Contains as data the address where the next WRITE_FLASH_DATA_COMMAND calls will write data to . - Is followed by 4 calls to WRITE_FLASH_DATA_COMMAND because writes are by 32 bytes blocks . - Has a SID like this : 0x600 | (motod_id << 5) | 0x5 - Gets answered with the usual acknowledgement CAN message(same SID but with 0x10 bit on, same length, same data ) It triggers this code in the PIC18F bootloader : \ code /* This is setting up the flash addressing registers to some address received by a CAN command. This sets everything up for future writes. */ static void write_flash_address(void) { position = 0; // resets the position to 0, we are starting to write a 32 bytes block TBLPTRU = CanMsgR.d.byte[2]; TBLPTRH = CanMsgR.d.byte[1]; TBLPTRL = CanMsgR.d.byte[0]; } \ endcode \ subsection canbootloadercommandmagicpacket MAGIC_PACKET This command : - Has a length of 8 bytes. - Contains 0x55AA55AA55AA55AA - Has a SID like this : 0x600 | (motor_id << 5) | 0xA - Gets answered with the usual acknowledgement CAN message(same SID but with 0x10 bit on, same length, same data ) - It gets acknowledged at bootup of the PIC18F . It triggers this code in the PIC18F bootloader : \ code _asm goto 0x0000 _endasm \ endcode Note : if this packet is received by the SimpleMotor code (and*NOT by the bootloader code ), with the first two bits of the SID equal to "10" : Then before rebooting the PIC18F will erase the last byte of EEPROM, setting it to 0xFF. This is to force the bootloader to stay in bootloader mode upon reboot and not start directly the user application. \ section roscommunicationprotocol How ROS interacts with the PIC32 \ subsection roscommands ROS commands sent to the PIC32 ROS sends commands through the EtherCAT protocol using one Mailbox andone FMMU (which are setup in SR06::construct() ). The data are sent by the SR06::packCommand() function. The commands consist in 2 structures. The first one : \ code typedef struct { EDC_COMMAND EDC_command; int16s motor_torque_demand[20]; } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_INCOMING; \ endcode the EDC_COMMAND type being : \ code typedef enum { EDC_COMMAND_INVALID = 0, EDC_COMMAND_SENSOR_DATA, EDC_COMMAND_SENSOR_CHANNEL_NUMBERS, EDC_COMMAND_SENSOR_ADC_CHANNEL_CS, EDC_COMMAND_TEST_RESULTS, EDC_COMMAND_CAN_TEST_MODE } EDC_COMMAND; \ endcode This structure contains the torque demand that ROS sends to the PIC32 (which are afterward being forwarded through CAN by the PIC32 to the SimpleMotors ). The first member of the structure describes the operating mode ROS wants the PIC32 to be in. - 0 has been choosen to be an invalid command, since thanks to the SyncManager mechanism, reading a Mailbox that hasn't been written to yet, is read as all 0s. - The "normal" functionning mode is "EDC_COMMAND_SENSOR_DATA". In this mode the PIC32 behaviour is the one described in the Palm_EDC_Specification. odt Which means it sends the data request messages to the motors, reads ADC values(through SPI) sends torque demands to motors, sends back to ROS the Joints data andthe motor data . - The "EDC_COMMAND_SENSOR_CHANNEL_NUMBERS" command is like the "EDC_COMMAND_SENSOR_DATA" one but instead of sending back the joints data, the PIC32 sends back the Channel and ADC number of the joint, this is for tests purposes . - The "EDC_COMMAND_CAN_TEST_MODE" makes the PIC32 go into "CAN_TEST_MODE", so that it does not follow anymore the specification in Palm_EDC_Specification . odt anymore . - In this mode the PIC32 just acts as a CAN bridge between ROS andthe CAN bus . The second structure in the command is : \ code typedef struct { int8u can_bus; int8u message_length; int16u message_id; int8u message_data[8]; } __attribute__((packed)) ETHERCAT_CAN_BRIDGE_DATA; \ endcode This structure is filled with CAN messages to be sent by the PIC32 on the CAN bus . It is only usefull in CAN_TEST_MODE . The content of this structure will be ignored by the PIC32 if not in CAN_TEST_MODE . \ subsection rosstatus ROS status sent by the PIC32, received by ROS ROS receives data through the EtherCAT protocol using one Mailbox andone FMMU (which are setup in SR06::construct() ). The data are sent by the PIC32 andreceived by SR06::unpackState() function. Those data are called "status" in ROS . The status consist in 2 structures. The first one : \ code typedef struct { EDC_COMMAND EDC_command; int16s FFJ1, FFJ2, FFJ3, FFJ4; int16s MFJ1, MFJ2, MFJ3, MFJ4; int16s RFJ1, RFJ2, RFJ3, RFJ4; int16s LFJ1, LFJ2, LFJ3, LFJ4; int16s LFJ5, THJ1, THJ2, THJ3; int16s THJ4, THJ5, WRJ1, WRJ2; int16s ACCX, ACCY, ACCZ, GYRX; int16s GYRY, GYRZ, AN_0, AN_1; int16s AN_2, AN_3, nothing1, nothing2; MOTOR_DATA_OUT motor[20]; TACTILE_SENSOR_OUT tactile[5]; }__attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_OUTGOING; \ endcode With EDC_COMMAND being : \ code typedef enum { EDC_COMMAND_INVALID = 0, EDC_COMMAND_SENSOR_DATA, EDC_COMMAND_SENSOR_CHANNEL_NUMBERS, EDC_COMMAND_SENSOR_ADC_CHANNEL_CS, EDC_COMMAND_TEST_RESULTS, EDC_COMMAND_CAN_TEST_MODE } EDC_COMMAND;
MOTOR_DATA_OUT structure is defined by :
\ code
typedef struct { int16s torque; int16u SG_L; int16u SG_R; int16s temperature; int16s current; int16u flags; } MOTOR_DATA_OUT;
\
endcode
The second status :
the structure is the same as the one in the command :
\ code
typedef struct { int8u can_bus; int8u message_length; int16u message_id; int8u message_data[8]; } __attribute__((packed)) ETHERCAT_CAN_BRIDGE_DATA;
\
endcode
\ section todo TODO & Improvements
What 's next to be done ?
For the moment there is the possibility to put the PIC32 in "CAN_TEST_MODE", so that it will act as a CAN bridge between ROS andthe PIC18. Which means the PIC32 will read a structure in the "command" Mailbox (coming from ROS ) containing a CAN message to be sent. It will send the CAN message over the CAN busAnd every CAN message received by the PIC32 will be sent back to ROS through a structure in the "status" Mailbox (from the PIC32 to ROS ).
This is implemented in the PIC32 and has been tested (putting some test code in
and
which has been removed afterward) and works.
But there is no code at the moment in the last revision of SR06 ROS driver that gives a way to do that (telling the PIC32 to send a crafted CAN message )
What would be great to implement is a ROS service that would allow sending such a crafted CAN message.
doing something like
\ code
rosservice call "SendCanMessage" SID Length 8-bytes-long-content-in- hex
\ endcode
So that one could do tests running the service manually (typing the above command in a shell ) or doing automated test running a special test node that would call this service one or several times
For the moment there is the possibility to put the PIC32 in "SENSOR_CHANNEL_NUMBERS" mode, so that the PIC32 won't put the sensor (joints) data inside the structure which is sent back to ROS through the "status" Mailbox, but instead it would put the ADC number and channel number.
For example :
\
code
ROS_INFO("LFJ5 : 0x%04X", tbuffer->LFJ5);
\
endcode
would print "LFJ5 : 0x0605" while in that "SENSOR_CHANNEL_NUMBERS" mode, because LFJ5 's data comes from Channel 06 of ADC 05.
This is a "test" feature, it has been tested putting some code in
and
which has been removed afterward since it's not "normal functionning".
So it would be great to implement a service to enter that mode andtest if the data received are correct.
Doing something like
\ code
rosservice call "TestADCChannelNumbers"
The protocol described in https://github.com/shadow-robot/hand-firmware/blob/master/EDC/documentation/Palm_EDC_Specification.odt for the communication between PIC32 and PIC18F is quite messy and has been re-thought differently, using more efficiently the SID and in a more "standardized" way.
This new SID bit mapping is actually the one used by the PIC18F CAN bootloader, and which is described in the "CAN SID bit mapping" subsection of the "Bootloading protocole" section.
It would be great to change the PIC32 and SimpleMotor code to reflect this kind of new SID bit mapping. So that all the SIDs in all the CAN protocols used through the robot code would be homogeneous