00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00042
00043
00044 #ifndef PALM_EDC_0220_ETHERCAT_PROTOCOL_H_INCLUDED
00045 #define PALM_EDC_0220_ETHERCAT_PROTOCOL_H_INCLUDED
00046
00047 #define NUM_MOTORS 20
00048
00049
00051 typedef enum
00052 {
00053 EDC_COMMAND_INVALID = 0,
00054 EDC_COMMAND_SENSOR_DATA,
00055 EDC_COMMAND_SENSOR_CHANNEL_NUMBERS,
00056 EDC_COMMAND_SENSOR_ADC_CHANNEL_CS,
00057 EDC_COMMAND_CAN_DIRECT_MODE
00058
00059
00060 }EDC_COMMAND;
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00098 typedef enum
00099 {
00100 MOTOR_DATA_INVALID = 0x0,
00101 MOTOR_DATA_SGL = 0x1,
00102 MOTOR_DATA_SGR = 0x2,
00103 MOTOR_DATA_PWM = 0x3,
00104 MOTOR_DATA_FLAGS = 0x4,
00105 MOTOR_DATA_CURRENT = 0x5,
00106 MOTOR_DATA_VOLTAGE = 0x6,
00107 MOTOR_DATA_TEMPERATURE = 0x7,
00108 MOTOR_DATA_CAN_NUM_RECEIVED = 0x8,
00109 MOTOR_DATA_CAN_NUM_TRANSMITTED = 0x9,
00110 MOTOR_DATA_SLOW_MISC = 0xA,
00111 MOTOR_DATA_READBACK_LAST_CONFIG = 0xB,
00112 MOTOR_DATA_CAN_ERROR_COUNTERS = 0xC,
00113
00114 MOTOR_DATA_PTERM = 0xD,
00115 MOTOR_DATA_ITERM = 0xE,
00116 MOTOR_DATA_DTERM = 0xF
00117 }FROM_MOTOR_DATA_TYPE;
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 typedef enum
00128 {
00129 MOTOR_SLOW_DATA_INVALID = 0x0000,
00130 MOTOR_SLOW_DATA_SVN_REVISION = 0x0001,
00131 MOTOR_SLOW_DATA_SVN_SERVER_REVISION = 0x0002,
00132
00133 MOTOR_SLOW_DATA_SVN_MODIFIED = 0x0003,
00134 MOTOR_SLOW_DATA_SERIAL_NUMBER_LOW = 0x0004,
00135 MOTOR_SLOW_DATA_SERIAL_NUMBER_HIGH = 0x0005,
00136 MOTOR_SLOW_DATA_GEAR_RATIO = 0x0006,
00137 MOTOR_SLOW_DATA_ASSEMBLY_DATE_YYYY = 0x0007,
00138 MOTOR_SLOW_DATA_ASSEMBLY_DATE_MMDD = 0x0008,
00139
00140 MOTOR_SLOW_DATA_CONTROLLER_F = 0x0009,
00141 MOTOR_SLOW_DATA_CONTROLLER_P = 0x000A,
00142 MOTOR_SLOW_DATA_CONTROLLER_I = 0x000B,
00143 MOTOR_SLOW_DATA_CONTROLLER_IMAX = 0x000C,
00144 MOTOR_SLOW_DATA_CONTROLLER_D = 0x000D,
00145 MOTOR_SLOW_DATA_CONTROLLER_DEADSIGN = 0x000E,
00146 MOTOR_SLOW_DATA_CONTROLLER_FREQUENCY = 0x000F,
00147
00148 MOTOR_SLOW_DATA_STRAIN_GAUGE_TYPE = 0x0010,
00149
00150 MOTOR_SLOW_DATA_LAST = 0x0010
00151 }FROM_MOTOR_SLOW_DATA_TYPE;
00152
00153 #define STRAIN_GAUGE_TYPE_COUPLED 0x0C
00154 #define STRAIN_GAUGE_TYPE_DECOUPLED 0x0D
00155
00156 #ifndef NO_STRINGS // The PIC compiler doesn't deal well with strings.
00157
00158 static const char* slow_data_types[17] = { "Invalid",
00159 "SVN revision",
00160 "SVN revision on server at build time",
00161
00162 "Modified from SVN revision",
00163 "Serial number low",
00164 "Serial number high",
00165 "Motor gear ratio",
00166 "Assembly date year",
00167 "Assembly date month, day",
00168
00169 "Controller F",
00170 "Controller P",
00171 "Controller I",
00172 "Controller Imax",
00173 "Controller D",
00174 "Controller deadband and sign",
00175
00176 "Controller loop frequency Hz"
00177 };
00178
00179 #endif
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 #define MOTOR_FLAG_BITS_CURRENT_CHOKE 0x000F //!< Top 4 bits of the current choke.
00194
00195 #define MOTOR_FLAG_BITS_EEPROM_WRITING 0x0010 //!< 1=EEPROM write currently in progress, don't update configuration.
00196 #define MOTOR_FLAG_BITS_LAST_CONFIG_CRC_FAILED 0x0020 //!< 1=Last CRC message didn't patch the previously sent configs.
00197 #define MOTOR_FLAG_BITS_LAST_CONFIG_OUT_OF_RANGE 0x0040 //!< 1=Last config message contained a value which was out of range.
00198 #define MOTOR_FLAG_BITS_JIGGLING_IN_PROGRESS 0x0080 //!< Jiggling in progress to zreo gauge readings. Control unavailable at this time.
00199
00200
00201
00202
00203
00204 #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)
00205 #define MOTOR_FLAG_BITS_NO_DEMAND_SEEN 0x0400 //!< Haven't received any demand messages for longer than NO_DEMAND_TIMEOUT_MS. Motor halted
00206 #define MOTOR_FLAG_BITS_SGL_FAULT 0x0800 //!< Fault seen with Left strain gauge. Not currently implemented.
00207
00208 #define MOTOR_FLAG_BITS_SGR_FAULT 0x1000 //!< Fault seen with Left strain gauge. Not currently implemented.
00209 #define MOTOR_FLAG_BITS_A3950_NFAULT 0x2000 //!< nFault output from A3950 H-Bridge
00210 #define MOTOR_FLAG_BITS_EEPROM_CONFIG_BAD_CRC 0x4000 //!< EEPROM contains a bad CRC. Configs not loaded.
00211 #define MOTOR_FLAG_BITS_OVER_TEMP 0x8000 //!< Motor over-heated and halted.
00212
00213
00214 #define NO_TORQUE_CONTROL_ERROR_FLAGS ( MOTOR_FLAG_BITS_SGL_FAULT \
00215 | MOTOR_FLAG_BITS_SGR_FAULT )
00216
00217 #define SERIOUS_ERROR_FLAGS ( MOTOR_FLAG_BITS_NO_DEMAND_SEEN \
00218 | MOTOR_FLAG_BITS_A3950_NFAULT \
00219 | MOTOR_FLAG_BITS_EEPROM_CONFIG_BAD_CRC \
00220 | MOTOR_FLAG_BITS_OVER_TEMP )
00221
00222
00223 #define NO_DEMAND_TIMEOUT_MS 20 //!< If a motor doesn't see any Torque or PWM demand values,
00224
00225
00226 #ifndef NO_STRINGS // The PIC compiler doesn't deal well with strings.
00227
00230 static const char* error_flag_names[16] = { "Current choke bit 6",
00231 "Current choke bit 7",
00232 "Current choke bit 8",
00233 "Current choke bit 9",
00234
00235 "EEPROM write in progress",
00236 "Last CRC sent didn't match configs",
00237 "Last config received was out of range",
00238 "Jiggling in progress",
00239
00240 "Invalid flag",
00241 "Motor ID is invalid",
00242 "No demand seen for more than 20ms",
00243 "Fault with strain gauge 0: left",
00244
00245 "Fault with strain gauge 1: right",
00246 "A3950 H-bridge nFault asserted",
00247 "EEPROM contains bad CRC. Motor off.",
00248 "Motor over temperature"
00249 };
00250 #endif
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00266 typedef enum
00267 {
00268 MOTOR_DEMAND_INVALID = 0x0,
00269 MOTOR_DEMAND_TORQUE = 0x1,
00270 MOTOR_DEMAND_PWM = 0x2,
00271
00272
00273 MOTOR_SYSTEM_RESET = 0x3,
00274 MOTOR_SYSTEM_CONTROLS = 0x4,
00275
00276 MOTOR_CONFIG_FIRST_VALUE = 0x7,
00277 MOTOR_CONFIG_MAX_PWM = 0x7,
00278 MOTOR_CONFIG_SG_REFS = 0x8,
00279 MOTOR_CONFIG_F = 0x9,
00280 MOTOR_CONFIG_P = 0xA,
00281 MOTOR_CONFIG_I = 0xB,
00282 MOTOR_CONFIG_D = 0xC,
00283 MOTOR_CONFIG_IMAX = 0xD,
00284 MOTOR_CONFIG_DEADBAND_SIGN = 0xE,
00285 MOTOR_CONFIG_LAST_VALUE = 0xE,
00286 MOTOR_CONFIG_CRC = 0xF
00287
00288 }TO_MOTOR_DATA_TYPE;
00289
00290 #define MOTOR_SYSTEM_RESET_KEY 0x5200 //!< | Motor ID.
00291
00292 #define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE 0x0001 //!< Turn on Backlash Compensation
00293 #define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE 0x0002 //!< Turn off Backlash Compensation
00294 #define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC 0x0004 //!< Increment the tracking value for Left gauge
00295 #define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC 0x0008 //!< Decrement the tracking value for Left gauge
00296 #define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC 0x0010 //!< Increment the tracking value for Right gauge
00297 #define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC 0x0020 //!< Decrement the tracking value for Right gauge
00298 #define MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING 0x0040 //!< Initiate the jiggling to re-zero the strain gauges. You'll
00299
00300 #define MOTOR_SYSTEM_CONTROL_EEPROM_WRITE 0x0080 //!< Write the configuration to the EEPROM.
00301
00302
00303
00304 #define MOTOR_DEMAND_TORQUE_RANGE_MIN -0x7FFF
00305 #define MOTOR_DEMAND_TORQUE_RANGE_MAX 0x7FFF
00306
00307 #define MOTOR_DEMAND_PWM_RANGE_MIN -0x03FF
00308 #define MOTOR_DEMAND_PWM_RANGE_MAX 0x03FF
00309
00310 #define MOTOR_CONFIG_F_RANGE_MIN 0x0000
00311 #define MOTOR_CONFIG_F_RANGE_MAX 0x7FFF
00312
00313 #define MOTOR_CONFIG_P_RANGE_MIN 0x0000
00314 #define MOTOR_CONFIG_P_RANGE_MAX 0x7FFF
00315
00316 #define MOTOR_CONFIG_I_RANGE_MIN 0x0000
00317 #define MOTOR_CONFIG_I_RANGE_MAX 0x7FFF
00318
00319 #define MOTOR_CONFIG_D_RANGE_MIN 0x0000
00320 #define MOTOR_CONFIG_D_RANGE_MAX 0x7FFF
00321
00322 #define MOTOR_CONFIG_IMAX_RANGE_MIN 0x0000
00323 #define MOTOR_CONFIG_IMAX_RANGE_MAX 0x3FFF
00324
00325 #define MOTOR_CONFIG_DEADBAND_RANGE_MIN 0x00
00326 #define MOTOR_CONFIG_DEADBAND_RANGE_MAX 0xFF
00327
00328 #define MOTOR_CONFIG_SIGN_RANGE_MIN 0x00
00329 #define MOTOR_CONFIG_SIGN_RANGE_MAX 0x01
00330
00331
00332 #ifndef NO_STRINGS // The PIC compiler doesn't deal well with strings.
00333
00336 static const char* to_motor_data_type_names[16] = { "INVALID",
00337 "Demand: Torque",
00338 "Demand: PWM",
00339 "INVALID",
00340 "INVALID",
00341 "INVALID",
00342 "INVALID",
00343 "Config: Maximum PWM value",
00344 "Config: Strain gauge amp references. LSB = ref for SGL, MSB = ref for SGR",
00345 "Config: Feed forward gain",
00346 "Config: Proportional gain",
00347 "Config: Integral gain",
00348 "Config: Derivative gain",
00349 "Config: Maximum integral windup",
00350 "Config: MSB=Deadband. LSB=Sign",
00351 "Config: CRC",
00352 };
00353 #endif
00354
00355
00362 typedef enum
00363 {
00364 MOTOR_DEBUG_INVALID = 0x0,
00365 MOTOR_DEBUG_TORQUE_DEMAND = 0x1,
00366 MOTOR_DEBUG_SGL = 0x2,
00367 MOTOR_DEBUG_SGR = 0x3,
00368 MOTOR_DEBUG_CURRENT = 0x4,
00369 MOTOR_DEBUG_TEMPERATURE = 0x5
00370 }TO_MOTOR_DEBUG_TYPE;
00371
00372
00373
00376 typedef struct
00377 {
00378 int16s torque;
00379 int16u misc;
00380 }MOTOR_DATA_PACKET;
00381
00382
00385 typedef enum
00386 {
00387 DIRECTION_DATA_REQUEST = 0x0,
00388 DIRECTION_TO_MOTOR = 0x1,
00389 DIRECTION_FROM_MOTOR = 0x2,
00390 DIRECTION_BOOTLOADER = 0x3
00391 }MESSAGE_DIRECTION;
00392
00393
00394 #define MESSAGE_ID_DIRECTION_BITS 0b11000000000 //!< Bit mask specifying which bits of the CAN message ID are used for the MESSAGE_DIRECTION
00395 #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]
00396 #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)
00397 #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
00398
00399 #define MESSAGE_ID_DIRECTION_SHIFT_POS 9 //!< Bit number of lowest bit of MESSAGE_ID_DIRECTION_BITS
00400 #define MESSAGE_ID_MOTOR_ID_SHIFT_POS 5 //!< Bit number of lowest bit of MESSAGE_ID_MOTOR_ID_BITS
00401 #define MESSAGE_ID_MOTOR_ID_BOTTOM_BIT 0b00000100000 //!< Used to mask for even/odd motors
00402 #define MESSAGE_ID_MOTOR_ID_TOP_2_BITS 0b00110000000 //!< Used to group motors into fours.
00403
00404
00405
00406 #define SENSORS_NUM_0220 ((int)36) //!< The number of sensors in the robot.
00407
00408
00409 #define JOINTS_NUM_0220 ((int)28) //!< The number of joints in the hand
00410
00411
00412 #ifndef NO_STRINGS // The PIC compiler doesn't deal well with strings.
00413
00414 static const char* joint_names[JOINTS_NUM_0220] = { "FFJ0", "FFJ1", "FFJ2", "FFJ3", "FFJ4",
00415 "MFJ0", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
00416 "RFJ0", "RFJ1", "RFJ2", "RFJ3", "RFJ4",
00417 "LFJ0", "LFJ1", "LFJ2", "LFJ3", "LFJ4","LFJ5",
00418 "THJ1", "THJ2", "THJ3", "THJ4", "THJ5",
00419 "WRJ1", "WRJ2"
00420 };
00421
00422
00424
00425 static const char* sensor_names[SENSORS_NUM_0220] = {"FFJ1", "FFJ2", "FFJ3", "FFJ4",
00426 "MFJ1", "MFJ2", "MFJ3", "MFJ4",
00427 "RFJ1", "RFJ2", "RFJ3", "RFJ4",
00428 "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
00429 "THJ1", "THJ2", "THJ3", "THJ4", "THJ5A", "THJ5B",
00430 "WRJ1A", "WRJ1B", "WRJ2",
00431 "ACCX", "ACCY", "ACCZ",
00432 "GYRX", "GYRY", "GYRZ",
00433 "AN0", "AN1", "AN2", "AN3"
00434 };
00435 #endif
00436
00437
00439 typedef enum
00440 {
00441 FFJ1=0, FFJ2, FFJ3, FFJ4,
00442 MFJ1, MFJ2, MFJ3, MFJ4,
00443 RFJ1, RFJ2, RFJ3, RFJ4,
00444 LFJ1, LFJ2, LFJ3, LFJ4, LFJ5,
00445 THJ1, THJ2, THJ3, THJ4, THJ5A, THJ5B,
00446 WRJ1A, WRJ1B, WRJ2,
00447
00448 ACCX, ACCY, ACCZ,
00449 GYRX, GYRY, GYRZ,
00450
00451 AN0, AN1, AN2, AN3,
00452 IGNORE
00453 }SENSOR_NAME_ENUM;
00454
00455
00456
00457
00458 typedef enum
00459 {
00460 PALM_SVN_VERSION = 0,
00461 SERVER_SVN_VERSION = 1
00462 }HARD_CONFIGURATION_INFORMATION;
00463
00464
00465
00466
00467
00468
00469
00470 #define TACTILE_DATA_LENGTH_BYTES 16
00471 #define TACTILE_DATA_LENGTH_WORDS (TACTILE_DATA_LENGTH_BYTES/2)
00472
00473 typedef union
00474 {
00475 int16u word[TACTILE_DATA_LENGTH_WORDS];
00476 char string[TACTILE_DATA_LENGTH_BYTES];
00477 }TACTILE_SENSOR_STATUS;
00478
00479 typedef enum
00480 {
00481 TACTILE_SENSOR_TYPE_WHICH_SENSORS = 0xFFF9,
00482 TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ = 0xFFFA,
00483 TACTILE_SENSOR_TYPE_MANUFACTURER = 0xFFFB,
00484 TACTILE_SENSOR_TYPE_SERIAL_NUMBER = 0xFFFC,
00485 TACTILE_SENSOR_TYPE_SOFTWARE_VERSION = 0xFFFD,
00486 TACTILE_SENSOR_TYPE_PCB_VERSION = 0xFFFE,
00487 TACTILE_SENSOR_TYPE_RESET_COMMAND = 0xFFFF
00488 }FROM_TACTILE_SENSOR_TYPE;
00489
00490
00491 typedef enum
00492 {
00493 TACTILE_SENSOR_PROTOCOL_TYPE_INVALID = 0x0000,
00494 TACTILE_SENSOR_PROTOCOL_TYPE_PST3 = 0x0001,
00495 TACTILE_SENSOR_PROTOCOL_TYPE_BIOTAC_2_3 = 0x0002,
00496
00497 TACTILE_SENSOR_PROTOCOL_TYPE_CONFLICTING = 0xFFFF
00498 }TACTILE_SENSOR_PROTOCOL_TYPE;
00499
00500
00501 typedef enum
00502 {
00503 TACTILE_SENSOR_TYPE_PST3_PRESSURE_TEMPERATURE = 0x0000,
00504 TACTILE_SENSOR_TYPE_PST3_PRESSURE_RAW_ZERO_TRACKING = 0x0002,
00505 TACTILE_SENSOR_TYPE_PST3_DAC_VALUE = 0x0004
00506 }FROM_TACTILE_SENSOR_TYPE_PST3;
00507
00508
00509 typedef enum
00510 {
00511 TACTILE_SENSOR_TYPE_BIOTAC_INVALID = 0x0000,
00512 TACTILE_SENSOR_TYPE_BIOTAC_PDC = 0x0001,
00513 TACTILE_SENSOR_TYPE_BIOTAC_TAC = 0x0002,
00514 TACTILE_SENSOR_TYPE_BIOTAC_TDC = 0x0003,
00515 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1 = 0x0004,
00516 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2 = 0x0005,
00517 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3 = 0x0006,
00518 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4 = 0x0007,
00519 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5 = 0x0008,
00520 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6 = 0x0009,
00521 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7 = 0x000A,
00522 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8 = 0x000B,
00523 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9 = 0x000C,
00524 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10 = 0x000D,
00525 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11 = 0x000E,
00526 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12 = 0x000F,
00527 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13 = 0x0010,
00528 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14 = 0x0011,
00529 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15 = 0x0012,
00530 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16 = 0x0013,
00531 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17 = 0x0014,
00532 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18 = 0x0015,
00533 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19 = 0x0016,
00534
00535 FROM_TACTILE_SENSOR_TYPE_BIOTAC_NUM_VALUES = 0x0017
00536 }FROM_TACTILE_SENSOR_TYPE_BIOTAC;
00537
00538
00539 typedef struct
00540 {
00541 int16u Pac[2];
00542 int16u other_sensor;
00543 struct
00544 {
00545 int8u Pac0 : 1;
00546 int8u Pac1 : 1;
00547 int8u other_sensor : 1;
00548 }data_valid;
00549
00550 int16u nothing[4];
00551 }TACTILE_SENSOR_BIOTAC_DATA_CONTENTS;
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00588 typedef struct
00589 {
00590 EDC_COMMAND EDC_command;
00591
00592
00593
00594
00595 int16u sensors[SENSORS_NUM_0220+1];
00596
00597 FROM_MOTOR_DATA_TYPE motor_data_type;
00598
00599
00600 int16s which_motors;
00601
00602
00603
00604 int32u which_motor_data_arrived;
00605 int32u which_motor_data_had_errors;
00606
00607 MOTOR_DATA_PACKET motor_data_packet[10];
00608
00609 int32u tactile_data_type;
00610 int16u tactile_data_valid;
00611 TACTILE_SENSOR_STATUS tactile[5];
00612
00613 int16u idle_time_us;
00614
00615
00616 } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS;
00617
00618
00619
00621 typedef struct
00622 {
00623 EDC_COMMAND EDC_command;
00624
00625 FROM_MOTOR_DATA_TYPE from_motor_data_type;
00626 int16s which_motors;
00627
00628
00629 TO_MOTOR_DATA_TYPE to_motor_data_type;
00630 int16s motor_data[NUM_MOTORS];
00631
00632 int32u tactile_data_type;
00633
00634 } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND;
00635
00636
00637
00638
00639
00640
00641
00642 #define ETHERCAT_COMMAND_HEADER_SIZE ( sizeof(EDC_COMMAND) + sizeof(FROM_MOTOR_DATA_TYPE) + sizeof(int16s) )
00643
00644 #define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS)
00645 #define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND)
00646
00647
00652 typedef struct
00653 {
00654 int8u can_bus;
00655 int8u message_length;
00656 int16u message_id;
00657 int8u message_data[8];
00658 } __attribute__((packed)) ETHERCAT_CAN_BRIDGE_DATA;
00659
00660 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
00661
00662
00674
00675 #define ETHERCAT_COMMAND_DATA_ADDRESS 0x1000
00676 #define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS (ETHERCAT_COMMAND_DATA_ADDRESS + ETHERCAT_COMMAND_DATA_SIZE)
00677
00678 #define ETHERCAT_STATUS_DATA_ADDRESS (ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS + ETHERCAT_CAN_BRIDGE_DATA_SIZE)
00679 #define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS (ETHERCAT_STATUS_DATA_ADDRESS + ETHERCAT_STATUS_DATA_SIZE)
00680
00681
00682
00683 #define INSERT_CRC_CALCULATION_HERE crc_i = (int8u) (crc_result&0xff); \
00684 crc_i ^= crc_byte; \
00685 crc_result >>= 8; \
00686 if(crc_i & 0x01) crc_result ^= 0x3096; \
00687 if(crc_i & 0x02) crc_result ^= 0x612c; \
00688 if(crc_i & 0x04) crc_result ^= 0xc419; \
00689 if(crc_i & 0x08) crc_result ^= 0x8832; \
00690 if(crc_i & 0x10) crc_result ^= 0x1064; \
00691 if(crc_i & 0x20) crc_result ^= 0x20c8; \
00692 if(crc_i & 0x40) crc_result ^= 0x4190; \
00693 if(crc_i & 0x80) crc_result ^= 0x8320;
00694
00695 #endif