sr_edc_ethercat_drivers Documentation

sr_edc_ethercat_drivers

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.

Where to find what ?

How to burn the PIC18F CAN bootloader ?

How to flash a firmware using the CAN bootloader ?

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.

How to do a bootloader-aware firmware ?

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

SR06::packCommand()

and

SR06::unpackState()

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

SR06::packCommand()

and

SR06::unpackState()

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

  1. The two first bits can stay as "input"/"output" bits, the following 4 bits can stay as "motor_id" bits, the bit 0x10 can stay as an ACK bit if ACK is needed, and the last 4 bits can contain the "data requested" in case of a "request data message" ( as explained in the Palm_EDC_Specification.odt).
  2. That would save using 2 bytes in the can datas for the "which_data" field, just putting this in the last 4 bits of the SID
  1. If there is too much data types to ask, we could use the last 5 bits of the SID(so one extra bit), since there is no ACK in this protocol anyway

sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Tue Oct 13 2020 04:02:02