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 Mon Jul 1 2019 20:06:31