Go to the source code of this file.
The term "Command" means data going from the ROS PC to the Node on the robot Previously known as "Incoming"
The term "Status" means data going from Node on the robot the to the ROS PC Previously known as "Outgoing"
Definition in file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS (ETHERCAT_COMMAND_DATA_ADDRESS + ETHERCAT_COMMAND_DATA_SIZE) |
Definition at line 676 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA) |
Definition at line 660 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS (ETHERCAT_STATUS_DATA_ADDRESS + ETHERCAT_STATUS_DATA_SIZE) |
Definition at line 679 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_COMMAND_DATA_ADDRESS 0x1000 |
| ETHERCAT_COMMAND_DATA | ETHERCAT_CAN_BRIDGE_DATA_COMMAND | ETHERCAT_STATUS_DATA | ETHERCAT_CAN_BRIDGE_DATA_STATUS | | | | | | | | ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS | | | | | ETHERCAT_STATUS_DATA_ADDRESS | | | ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS | ETHERCAT_COMMAND_DATA_ADDRESS
Definition at line 675 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND) |
Definition at line 645 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_COMMAND_HEADER_SIZE ( sizeof(EDC_COMMAND) + sizeof(FROM_MOTOR_DATA_TYPE) + sizeof(int16s) ) |
Definition at line 642 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_STATUS_DATA_ADDRESS (ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS + ETHERCAT_CAN_BRIDGE_DATA_SIZE) |
Definition at line 678 of file 0220_palm_edc_ethercat_protocol.h.
#define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS) |
Definition at line 644 of file 0220_palm_edc_ethercat_protocol.h.
#define INSERT_CRC_CALCULATION_HERE |
crc_i = (int8u) (crc_result&0xff); \ crc_i ^= crc_byte; \ crc_result >>= 8; \ if(crc_i & 0x01) crc_result ^= 0x3096; \ if(crc_i & 0x02) crc_result ^= 0x612c; \ if(crc_i & 0x04) crc_result ^= 0xc419; \ if(crc_i & 0x08) crc_result ^= 0x8832; \ if(crc_i & 0x10) crc_result ^= 0x1064; \ if(crc_i & 0x20) crc_result ^= 0x20c8; \ if(crc_i & 0x40) crc_result ^= 0x4190; \ if(crc_i & 0x80) crc_result ^= 0x8320;
Definition at line 683 of file 0220_palm_edc_ethercat_protocol.h.
#define JOINTS_NUM_0220 ((int)28) |
This needs to be a #define because it's used to dimension an array.
The number of joints in the hand
Definition at line 409 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_ACK_BIT 0b00000010000 |
Bit mask specifying which bits of the CAN message ID are used for the ACK bit (only for bootloading)
Definition at line 396 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_DIRECTION_BITS 0b11000000000 |
Bit mask specifying which bits of the CAN message ID are used for the MESSAGE_DIRECTION.
Definition at line 394 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_DIRECTION_SHIFT_POS 9 |
Bit number of lowest bit of MESSAGE_ID_DIRECTION_BITS.
Definition at line 399 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_MOTOR_ID_BITS 0b00111100000 |
Bit mask specifying which bits of the CAN message ID are used for the motor ID [0..9].
Definition at line 395 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_MOTOR_ID_BOTTOM_BIT 0b00000100000 |
Used to mask for even/odd motors.
Definition at line 401 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_MOTOR_ID_SHIFT_POS 5 |
Bit number of lowest bit of MESSAGE_ID_MOTOR_ID_BITS.
Definition at line 400 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_MOTOR_ID_TOP_2_BITS 0b00110000000 |
Used to group motors into fours.
Definition at line 402 of file 0220_palm_edc_ethercat_protocol.h.
#define MESSAGE_ID_TYPE_BITS 0b00000001111 |
Bit mask specifying which bits of the CAN message ID are used for the TO_MOTOR_DATA_TYPE or FROM_MOTOR_DATA_TYPE.
Definition at line 397 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_D_RANGE_MAX 0x7FFF |
Definition at line 320 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_D_RANGE_MIN 0x0000 |
Definition at line 319 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_DEADBAND_RANGE_MAX 0xFF |
Definition at line 326 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_DEADBAND_RANGE_MIN 0x00 |
Definition at line 325 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_F_RANGE_MAX 0x7FFF |
Definition at line 311 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_F_RANGE_MIN 0x0000 |
Definition at line 310 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_I_RANGE_MAX 0x7FFF |
Definition at line 317 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_I_RANGE_MIN 0x0000 |
Definition at line 316 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_IMAX_RANGE_MAX 0x3FFF |
Definition at line 323 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_IMAX_RANGE_MIN 0x0000 |
Definition at line 322 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_P_RANGE_MAX 0x7FFF |
Definition at line 314 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_P_RANGE_MIN 0x0000 |
Definition at line 313 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_SIGN_RANGE_MAX 0x01 |
Definition at line 329 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_CONFIG_SIGN_RANGE_MIN 0x00 |
Definition at line 328 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_DEMAND_PWM_RANGE_MAX 0x03FF |
Definition at line 308 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_DEMAND_PWM_RANGE_MIN -0x03FF |
Definition at line 307 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_DEMAND_TORQUE_RANGE_MAX 0x7FFF |
Definition at line 305 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_DEMAND_TORQUE_RANGE_MIN -0x7FFF |
Definition at line 304 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_A3950_NFAULT 0x2000 |
nFault output from A3950 H-Bridge
Definition at line 209 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_CURRENT_CHOKE 0x000F |
Top 4 bits of the current choke.
Definition at line 193 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_EEPROM_CONFIG_BAD_CRC 0x4000 |
EEPROM contains a bad CRC. Configs not loaded.
Definition at line 210 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_EEPROM_WRITING 0x0010 |
1=EEPROM write currently in progress, don't update configuration.
Definition at line 195 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_JIGGLING_IN_PROGRESS 0x0080 |
Jiggling in progress to zreo gauge readings. Control unavailable at this time.
Definition at line 198 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_LAST_CONFIG_CRC_FAILED 0x0020 |
1=Last CRC message didn't patch the previously sent configs.
Definition at line 196 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_LAST_CONFIG_OUT_OF_RANGE 0x0040 |
1=Last config message contained a value which was out of range.
Definition at line 197 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_MOTOR_ID_IS_INVALID 0x0200 |
Motor seems to have an out-of-range ID. You'll probably never see this flag. (Might get rid of it)
Definition at line 204 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_NO_DEMAND_SEEN 0x0400 |
Haven't received any demand messages for longer than NO_DEMAND_TIMEOUT_MS. Motor halted.
Definition at line 205 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_OVER_TEMP 0x8000 |
Motor over-heated and halted.
Definition at line 211 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_SGL_FAULT 0x0800 |
Fault seen with Left strain gauge. Not currently implemented.
Definition at line 206 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_FLAG_BITS_SGR_FAULT 0x1000 |
Fault seen with Left strain gauge. Not currently implemented.
Definition at line 208 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE 0x0002 |
Turn off Backlash Compensation.
Definition at line 293 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE 0x0001 |
Turn on Backlash Compensation.
Definition at line 292 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_EEPROM_WRITE 0x0080 |
see the MOTOR_FLAG_BITS_JIGGLING_IN_PROGRESS flag appear.
Write the configuration to the EEPROM.
Definition at line 300 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING 0x0040 |
Initiate the jiggling to re-zero the strain gauges. You'll
Definition at line 298 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC 0x0008 |
Decrement the tracking value for Left gauge.
Definition at line 295 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC 0x0004 |
Increment the tracking value for Left gauge.
Definition at line 294 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC 0x0020 |
Decrement the tracking value for Right gauge.
Definition at line 297 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC 0x0010 |
Increment the tracking value for Right gauge.
Definition at line 296 of file 0220_palm_edc_ethercat_protocol.h.
#define MOTOR_SYSTEM_RESET_KEY 0x5200 |
| Motor ID.
Definition at line 290 of file 0220_palm_edc_ethercat_protocol.h.
#define NO_DEMAND_TIMEOUT_MS 20 |
If a motor doesn't see any Torque or PWM demand values,
Definition at line 223 of file 0220_palm_edc_ethercat_protocol.h.
#define NO_TORQUE_CONTROL_ERROR_FLAGS |
Definition at line 214 of file 0220_palm_edc_ethercat_protocol.h.
#define NUM_MOTORS 20 |
Definition at line 47 of file 0220_palm_edc_ethercat_protocol.h.
#define SENSORS_NUM_0220 ((int)36) |
The number of sensors in the robot.
Definition at line 406 of file 0220_palm_edc_ethercat_protocol.h.
#define SERIOUS_ERROR_FLAGS |
( MOTOR_FLAG_BITS_NO_DEMAND_SEEN \ | MOTOR_FLAG_BITS_A3950_NFAULT \ | MOTOR_FLAG_BITS_EEPROM_CONFIG_BAD_CRC \ | MOTOR_FLAG_BITS_OVER_TEMP )
Definition at line 217 of file 0220_palm_edc_ethercat_protocol.h.
#define STRAIN_GAUGE_TYPE_COUPLED 0x0C |
Definition at line 153 of file 0220_palm_edc_ethercat_protocol.h.
#define STRAIN_GAUGE_TYPE_DECOUPLED 0x0D |
Definition at line 154 of file 0220_palm_edc_ethercat_protocol.h.
#define TACTILE_DATA_LENGTH_BYTES 16 |
Definition at line 470 of file 0220_palm_edc_ethercat_protocol.h.
#define TACTILE_DATA_LENGTH_WORDS (TACTILE_DATA_LENGTH_BYTES/2) |
Definition at line 471 of file 0220_palm_edc_ethercat_protocol.h.
enum EDC_COMMAND |
The host can request different types of data from the palm.
Definition at line 51 of file 0220_palm_edc_ethercat_protocol.h.
enum FROM_MOTOR_DATA_TYPE |
The host can request different types of data from the motors. These values are inserted into bits [3..0] of the message ID in the Data Request (Start Of Frame) message.
//!Word 0 | Word 1 | |||
FROM_MOTOR_DATA_TYPE | Byte0 | Byte1 | Byte2 | Byte3 |
MOTOR_DATA_SGL | Torque | SGL | ||
MOTOR_DATA_SGR | Torque | SGR | ||
MOTOR_DATA_PWM | Torque | PWM | ||
MOTOR_DATA_FLAGS | Torque | Flags | ||
MOTOR_DATA_CURRENT | Torque | Current | ||
MOTOR_DATA_VOLTAGE | Torque | Voltage | ||
MOTOR_DATA_TEMPERATURE | Torque | Temperature | ||
MOTOR_DATA_CAN_NUM_RECEIVED | Torque | Num Rx | ||
MOTOR_DATA_CAN_NUM_TRANSMITTED | Torque | Num Tx | ||
MOTOR_DATA_SVN_REVISION | Server revision | This revision (top bit = modified) | ||
MOTOR_DATA_READBACK_LAST_CONFIG | FROM_MOTOR_SLOW_DATA_TYPE | Config value | ||
MOTOR_DATA_CAN_ERROR_COUNTERS | Torque | Tx Err | Rx Err |
Definition at line 98 of file 0220_palm_edc_ethercat_protocol.h.
Definition at line 127 of file 0220_palm_edc_ethercat_protocol.h.
Definition at line 479 of file 0220_palm_edc_ethercat_protocol.h.
Definition at line 509 of file 0220_palm_edc_ethercat_protocol.h.
Definition at line 501 of file 0220_palm_edc_ethercat_protocol.h.
Definition at line 458 of file 0220_palm_edc_ethercat_protocol.h.
enum MESSAGE_DIRECTION |
This represents the top two bits [10..9] of the CAN message ID. These bits tell us the type of the message.
Definition at line 385 of file 0220_palm_edc_ethercat_protocol.h.
enum SENSOR_NAME_ENUM |
This enum defines which ADC reading goes into which sensors[].
FFJ1 | |
FFJ2 | |
FFJ3 | |
FFJ4 | |
MFJ1 | |
MFJ2 | |
MFJ3 | |
MFJ4 | |
RFJ1 | |
RFJ2 | |
RFJ3 | |
RFJ4 | |
LFJ1 | |
LFJ2 | |
LFJ3 | |
LFJ4 | |
LFJ5 | |
THJ1 | |
THJ2 | |
THJ3 | |
THJ4 | |
THJ5A | |
THJ5B | |
WRJ1A | |
WRJ1B | |
WRJ2 | |
ACCX | |
ACCY | |
ACCZ | |
GYRX | |
GYRY | |
GYRZ | |
AN0 | |
AN1 | |
AN2 | |
AN3 | |
IGNORE |
Definition at line 439 of file 0220_palm_edc_ethercat_protocol.h.
Definition at line 491 of file 0220_palm_edc_ethercat_protocol.h.
enum TO_MOTOR_DATA_TYPE |
The host can send different types of data from the motors. These can be either control demands, or configurations. These values are inserted into bits [3..0] of the message ID in the Motor data message.
MOTOR_DEMAND_INVALID |
A zero is what happens if an EtherCAT packet doesn't get through, so it's considered a special case. |
MOTOR_DEMAND_TORQUE |
Demanding torque activates the Torque PID loop. |
MOTOR_DEMAND_PWM |
Demanding PWM bypasses the Torque PID loop, and gives the exact PWM you asked for except where |
MOTOR_SYSTEM_RESET |
Send with a demand value of 0x520x to reset motor x. |
MOTOR_SYSTEM_CONTROLS |
Various bits to switch on / off misc things. |
MOTOR_CONFIG_FIRST_VALUE |
This is the first TO_MOTOR_DATA_TYPE which is actually a configuration and should be stored in EEPROM. |
MOTOR_CONFIG_MAX_PWM |
Put an upper limit on the absolute value of the motor PWM. Range [0..0x03FF]. |
MOTOR_CONFIG_SG_REFS |
Strain gauge amp references. LSB = ref for SGL, MSB = ref for SGR. |
MOTOR_CONFIG_F |
Feed forward gain. |
MOTOR_CONFIG_P |
Proportional gain. |
MOTOR_CONFIG_I |
Integral gain. |
MOTOR_CONFIG_D |
Derivative gain. |
MOTOR_CONFIG_IMAX |
Maximum integral windup. |
MOTOR_CONFIG_DEADBAND_SIGN |
MSB=sign. LSB=Deadband. |
MOTOR_CONFIG_LAST_VALUE |
This is the last config (apart from the CRC, which is special) |
MOTOR_CONFIG_CRC |
Sending this value, if it matches the CRC of the configs above, causes the configs to take effect. |
Definition at line 266 of file 0220_palm_edc_ethercat_protocol.h.
enum TO_MOTOR_DEBUG_TYPE |
Then the motor is in debug mode, the host can send different types of spoof sensor data to the motors. These values are inserted into bits [3..0] of the message ID in the Motor data message.
NOT CURRENTLY IMPLEMENTED IN THE MOTORS.
Definition at line 362 of file 0220_palm_edc_ethercat_protocol.h.
const char* error_flag_names[16] [static] |
{ "Current choke bit 6", "Current choke bit 7", "Current choke bit 8", "Current choke bit 9", "EEPROM write in progress", "Last CRC sent didn't match configs", "Last config received was out of range", "Jiggling in progress", "Invalid flag", "Motor ID is invalid", "No demand seen for more than 20ms", "Fault with strain gauge 0: left", "Fault with strain gauge 1: right", "A3950 H-bridge nFault asserted", "EEPROM contains bad CRC. Motor off.", "Motor over temperature" }
how long, in milliseconds, before it switches off the motor.
These are the names of the bits in the MOTOR_DATA_FLAGS. error_flag_names[n] is the name of bit 'n' in MOTOR_DATA_FLAGS.
Definition at line 230 of file 0220_palm_edc_ethercat_protocol.h.
const char* joint_names[JOINTS_NUM_0220] [static] |
{ "FFJ0", "FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ0", "MFJ1", "MFJ2", "MFJ3", "MFJ4", "RFJ0", "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ0", "LFJ1", "LFJ2", "LFJ3", "LFJ4","LFJ5", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2" }
This needs to be a #define for symmetry with SENSORS_NUM.
Definition at line 414 of file 0220_palm_edc_ethercat_protocol.h.
const char* sensor_names[SENSORS_NUM_0220] [static] |
{"FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4", "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5A", "THJ5B", "WRJ1A", "WRJ1B", "WRJ2", "ACCX", "ACCY", "ACCZ", "GYRX", "GYRY", "GYRZ", "AN0", "AN1", "AN2", "AN3" }
This array defines the names of the joints. The names and order should match the enum SENSOR_NAMES_ENUM.
Definition at line 425 of file 0220_palm_edc_ethercat_protocol.h.
const char* slow_data_types[17] [static] |
{ "Invalid", "SVN revision", "SVN revision on server at build time", "Modified from SVN revision", "Serial number low", "Serial number high", "Motor gear ratio", "Assembly date year", "Assembly date month, day", "Controller F", "Controller P", "Controller I", "Controller Imax", "Controller D", "Controller deadband and sign", "Controller loop frequency Hz" }
Definition at line 158 of file 0220_palm_edc_ethercat_protocol.h.
const char* to_motor_data_type_names[16] [static] |
{ "INVALID", "Demand: Torque", "Demand: PWM", "INVALID", "INVALID", "INVALID", "INVALID", "Config: Maximum PWM value", "Config: Strain gauge amp references. LSB = ref for SGL, MSB = ref for SGR", "Config: Feed forward gain", "Config: Proportional gain", "Config: Integral gain", "Config: Derivative gain", "Config: Maximum integral windup", "Config: MSB=Deadband. LSB=Sign", "Config: CRC", }
These are the human-readable names of the different types of data. you can send to the motors.
Definition at line 336 of file 0220_palm_edc_ethercat_protocol.h.