0220_palm_edc_ethercat_protocol.h
Go to the documentation of this file.
00001 //
00002 // © 2010 Shadow Robot Company Limited.
00003 //
00004 // FileName:        this_node.h
00005 // Dependencies:
00006 // Processor:       PIC32
00007 // Compiler:        MPLAB® C32
00008 //
00009 //  +------------------------------------------------------------------------+
00010 //  | This file is part of The Shadow Robot PIC32 firmware code base.        |
00011 //  |                                                                        |
00012 //  | It is free software: you can redistribute it and/or modify             |
00013 //  | it under the terms of the GNU General Public License as published by   |
00014 //  | the Free Software Foundation, either version 3 of the License, or      |
00015 //  | (at your option) any later version.                                    |
00016 //  |                                                                        |
00017 //  | It is distributed in the hope that it will be useful,                  |
00018 //  | but WITHOUT ANY WARRANTY; without even the implied warranty of         |
00019 //  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          |
00020 //  | GNU General Public License for more details.                           |
00021 //  |                                                                        |
00022 //  | You should have received a copy of the GNU General Public License      |
00023 //  | along with this code repository. The text of the license can be found  |
00024 //  | in Pic32/License/gpl.txt. If not, see <http://www.gnu.org/licenses/>.  |
00025 //  +------------------------------------------------------------------------+
00026 //
00027 //
00028 //
00029 //  Doxygen
00030 //  -------
00031 //
00042 //
00043 
00044 #ifndef PALM_EDC_0220_ETHERCAT_PROTOCOL_H_INCLUDED
00045 #define PALM_EDC_0220_ETHERCAT_PROTOCOL_H_INCLUDED
00046 
00047 #include "../common/tactile_edc_ethercat_protocol.h"
00048 #include "../common/ethercat_can_bridge_protocol.h"
00049 #include "../common/common_edc_ethercat_protocol.h"
00050 
00051 
00052 #define NUM_MOTORS 20
00053 
00054 
00055 // ========================================================
00056 // 
00057 //       F R O M    M O T O R    D A T A    T Y P E
00058 // 
00059 // ========================================================
00060 
00061 
00088 typedef enum
00089 {
00090     MOTOR_DATA_INVALID                  = 0x0,                  
00091     MOTOR_DATA_SGL                      = 0x1,                  
00092     MOTOR_DATA_SGR                      = 0x2,                  
00093     MOTOR_DATA_PWM                      = 0x3,                  
00094     MOTOR_DATA_FLAGS                    = 0x4,                  
00095     MOTOR_DATA_CURRENT                  = 0x5,                  
00096     MOTOR_DATA_VOLTAGE                  = 0x6,                  
00097     MOTOR_DATA_TEMPERATURE              = 0x7,                  
00098     MOTOR_DATA_CAN_NUM_RECEIVED         = 0x8,                  
00099     MOTOR_DATA_CAN_NUM_TRANSMITTED      = 0x9,                  
00100     MOTOR_DATA_SLOW_MISC                = 0xA,                  
00101     MOTOR_DATA_READBACK_LAST_CONFIG     = 0xB,                  
00102     MOTOR_DATA_CAN_ERROR_COUNTERS       = 0xC,                  
00103 
00104     MOTOR_DATA_PTERM                    = 0xD,                  
00105     MOTOR_DATA_ITERM                    = 0xE,                  
00106     MOTOR_DATA_DTERM                    = 0xF                   
00107 }FROM_MOTOR_DATA_TYPE;
00108 
00109 
00110 // ========================================================
00111 // 
00112 //                 S L O W    D A T A
00113 // 
00114 // ========================================================
00115 
00116 
00117 typedef enum
00118 {
00119     MOTOR_SLOW_DATA_INVALID              = 0x0000,              
00120     MOTOR_SLOW_DATA_SVN_REVISION         = 0x0001,              
00121     MOTOR_SLOW_DATA_SVN_SERVER_REVISION  = 0x0002,              
00122 
00123     MOTOR_SLOW_DATA_SVN_MODIFIED         = 0x0003,              
00124     MOTOR_SLOW_DATA_SERIAL_NUMBER_LOW    = 0x0004,              
00125     MOTOR_SLOW_DATA_SERIAL_NUMBER_HIGH   = 0x0005,              
00126     MOTOR_SLOW_DATA_GEAR_RATIO           = 0x0006,              
00127     MOTOR_SLOW_DATA_ASSEMBLY_DATE_YYYY   = 0x0007,              
00128     MOTOR_SLOW_DATA_ASSEMBLY_DATE_MMDD   = 0x0008,              
00129 
00130     MOTOR_SLOW_DATA_CONTROLLER_F         = 0x0009,              
00131     MOTOR_SLOW_DATA_CONTROLLER_P         = 0x000A,              
00132     MOTOR_SLOW_DATA_CONTROLLER_I         = 0x000B,              
00133     MOTOR_SLOW_DATA_CONTROLLER_IMAX      = 0x000C,              
00134     MOTOR_SLOW_DATA_CONTROLLER_D         = 0x000D,              
00135     MOTOR_SLOW_DATA_CONTROLLER_DEADSIGN  = 0x000E,              
00136     MOTOR_SLOW_DATA_CONTROLLER_FREQUENCY = 0x000F,              
00137 
00138     MOTOR_SLOW_DATA_STRAIN_GAUGE_TYPE    = 0x0010,              
00139 
00140     MOTOR_SLOW_DATA_LAST                 = 0x0010               
00141 }FROM_MOTOR_SLOW_DATA_TYPE;
00142 
00143 #define STRAIN_GAUGE_TYPE_COUPLED       0x0C
00144 #define STRAIN_GAUGE_TYPE_DECOUPLED     0x0D
00145 
00146 
00147 // ========================================================
00148 // 
00149 //                       F L A G S
00150 // 
00151 // ========================================================
00152 
00153 
00154                                                                 // Non serious flags, Just for information. Control still works.
00155                                                                 // -------------------------------------------------------------
00156 #define MOTOR_FLAG_BITS_CURRENT_CHOKE               0x000F      //!< Top 4 bits of the current choke.
00157 
00158 #define MOTOR_FLAG_BITS_EEPROM_WRITING              0x0010      //!< 1=EEPROM write currently in progress, don't update configuration.
00159 #define MOTOR_FLAG_BITS_LAST_CONFIG_CRC_FAILED      0x0020      //!< 1=Last CRC message didn't patch the previously sent configs.
00160 #define MOTOR_FLAG_BITS_LAST_CONFIG_OUT_OF_RANGE    0x0040      //!< 1=Last config message contained a value which was out of range.
00161 #define MOTOR_FLAG_BITS_JIGGLING_IN_PROGRESS        0x0080      //!< Jiggling in progress to zreo gauge readings. Control unavailable at this time.
00162 
00163 
00164                                                                 // Serious flags cause the motor to be switched off
00165                                                                 // ------------------------------------------------
00166 //#define                                           0x0100
00167 #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)
00168 #define MOTOR_FLAG_BITS_NO_DEMAND_SEEN              0x0400      //!< Haven't received any demand messages for longer than NO_DEMAND_TIMEOUT_MS. Motor halted
00169 #define MOTOR_FLAG_BITS_SGL_FAULT                   0x0800      //!< Fault seen with Left strain gauge. Not currently implemented.
00170 
00171 #define MOTOR_FLAG_BITS_SGR_FAULT                   0x1000      //!< Fault seen with Left strain gauge. Not currently implemented.
00172 #define MOTOR_FLAG_BITS_A3950_NFAULT                0x2000      //!< nFault output from A3950 H-Bridge
00173 #define MOTOR_FLAG_BITS_EEPROM_CONFIG_BAD_CRC       0x4000      //!< EEPROM contains a bad CRC. Configs not loaded.
00174 #define MOTOR_FLAG_BITS_OVER_TEMP                   0x8000      //!< Motor over-heated and halted.
00175 
00176 
00177 #define NO_TORQUE_CONTROL_ERROR_FLAGS  (  MOTOR_FLAG_BITS_SGL_FAULT              \
00178                                         | MOTOR_FLAG_BITS_SGR_FAULT )
00179 
00180 #define PALM_0200_EDC_SERIOUS_ERROR_FLAGS            (  MOTOR_FLAG_BITS_NO_DEMAND_SEEN         \
00181                                                       | MOTOR_FLAG_BITS_A3950_NFAULT           \
00182                                                       | MOTOR_FLAG_BITS_EEPROM_CONFIG_BAD_CRC  \
00183                                                       | MOTOR_FLAG_BITS_OVER_TEMP    )
00184 
00185 
00186 #define PALM_0200_EDC_NO_DEMAND_TIMEOUT_MS    20                                                              //!< If a motor doesn't see any Torque or PWM demand values,
00187 
00188 
00189 #ifndef NO_STRINGS                                                                                                                                  // The PIC compiler doesn't deal well with strings.
00190 
00193     static const char* palm_0200_edc_error_flag_names[16] = { "Current choke bit 6",                          // 0x0001
00194                                                 "Current choke bit 7",                          // 0x0002
00195                                                 "Current choke bit 8",                          // 0x0004
00196                                                 "Current choke bit 9",                          // 0x0008
00197 
00198                                                 "EEPROM write in progress",                     // 0x0010
00199                                                 "Last CRC sent didn't match configs",           // 0x0020
00200                                                 "Last config received was out of range",        // 0x0040
00201                                                 "Jiggling in progress",                         // 0x0080
00202 
00203                                                 "Invalid flag",                                 // 0x0100
00204                                                 "Motor ID is invalid",                          // 0x0200
00205                                                 "No demand seen for more than 20ms",            // 0x0400
00206                                                 "Fault with strain gauge 0: left",              // 0x0800
00207 
00208                                                 "Fault with strain gauge 1: right",             // 0x1000
00209                                                 "A3950 H-bridge nFault asserted",               // 0x2000
00210                                                 "EEPROM contains bad CRC. Motor off.",          // 0x4000
00211                                                 "Motor over temperature"                        // 0x8000
00212                                                };
00213 #endif
00214 
00215 
00216 
00217 
00218 
00219 // ========================================================
00220 // 
00221 //         T O    M O T O R    D A T A    T Y P E
00222 // 
00223 // ========================================================
00224 
00229 typedef enum
00230 {
00231     MOTOR_DEMAND_INVALID                = 0x0,                  
00232     MOTOR_DEMAND_TORQUE                 = 0x1,                  
00233     MOTOR_DEMAND_PWM                    = 0x2,                  
00234 
00235 
00236     MOTOR_SYSTEM_RESET                  = 0x3,                  
00237     MOTOR_SYSTEM_CONTROLS               = 0x4,                  
00238 
00239     MOTOR_CONFIG_FIRST_VALUE            = 0x7,                  
00240     MOTOR_CONFIG_MAX_PWM                = 0x7,                  
00241     MOTOR_CONFIG_SG_REFS                = 0x8,                  
00242     MOTOR_CONFIG_F                      = 0x9,                  
00243     MOTOR_CONFIG_P                      = 0xA,                  
00244     MOTOR_CONFIG_I                      = 0xB,                  
00245     MOTOR_CONFIG_D                      = 0xC,                  
00246     MOTOR_CONFIG_IMAX                   = 0xD,                  
00247     MOTOR_CONFIG_DEADBAND_SIGN          = 0xE,                  
00248     MOTOR_CONFIG_LAST_VALUE             = 0xE,                  
00249     MOTOR_CONFIG_CRC                    = 0xF                   
00250 
00251 }TO_MOTOR_DATA_TYPE;
00252 
00253 #define MOTOR_SYSTEM_RESET_KEY                              0x5200              //!< | Motor ID.
00254 
00255 #define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE   0x0001              //!< Turn on  Backlash Compensation
00256 #define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE  0x0002              //!< Turn off Backlash Compensation
00257 #define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC               0x0004              //!< Increment the tracking value for Left gauge
00258 #define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC               0x0008              //!< Decrement the tracking value for Left gauge
00259 #define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC               0x0010              //!< Increment the tracking value for Right gauge
00260 #define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC               0x0020              //!< Decrement the tracking value for Right gauge
00261 #define MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING              0x0040              //!< Initiate the jiggling to re-zero the strain gauges. You'll
00262 
00263 #define MOTOR_SYSTEM_CONTROL_EEPROM_WRITE                   0x0080              //!< Write the configuration to the EEPROM.
00264 
00265 
00266 
00267 #define MOTOR_DEMAND_TORQUE_RANGE_MIN       -0x7FFF
00268 #define MOTOR_DEMAND_TORQUE_RANGE_MAX        0x7FFF
00269 
00270 #define MOTOR_DEMAND_PWM_RANGE_MIN          -0x03FF
00271 #define MOTOR_DEMAND_PWM_RANGE_MAX           0x03FF
00272 
00273 #define MOTOR_CONFIG_F_RANGE_MIN             0x0000
00274 #define MOTOR_CONFIG_F_RANGE_MAX             0x7FFF
00275 
00276 #define MOTOR_CONFIG_P_RANGE_MIN             0x0000
00277 #define MOTOR_CONFIG_P_RANGE_MAX             0x7FFF
00278 
00279 #define MOTOR_CONFIG_I_RANGE_MIN             0x0000
00280 #define MOTOR_CONFIG_I_RANGE_MAX             0x7FFF
00281 
00282 #define MOTOR_CONFIG_D_RANGE_MIN             0x0000
00283 #define MOTOR_CONFIG_D_RANGE_MAX             0x7FFF
00284 
00285 #define MOTOR_CONFIG_IMAX_RANGE_MIN          0x0000
00286 #define MOTOR_CONFIG_IMAX_RANGE_MAX          0x3FFF
00287 
00288 #define MOTOR_CONFIG_DEADBAND_RANGE_MIN      0x00
00289 #define MOTOR_CONFIG_DEADBAND_RANGE_MAX      0xFF
00290 
00291 #define MOTOR_CONFIG_SIGN_RANGE_MIN          0x00
00292 #define MOTOR_CONFIG_SIGN_RANGE_MAX          0x01
00293 
00294 
00295 #ifndef NO_STRINGS                                                                                                                          // The PIC compiler doesn't deal well with strings.
00296 
00299     static const char* to_motor_data_type_names[16] = { "INVALID",
00300                                                         "Demand: Torque",
00301                                                         "Demand: PWM",
00302                                                         "INVALID",
00303                                                         "INVALID",
00304                                                         "INVALID",
00305                                                         "INVALID",
00306                                                         "Config: Maximum PWM value",
00307                                                         "Config: Strain gauge amp references. LSB = ref for SGL, MSB = ref for SGR",
00308                                                         "Config: Feed forward gain",
00309                                                         "Config: Proportional gain",
00310                                                         "Config: Integral gain",
00311                                                         "Config: Derivative gain",
00312                                                         "Config: Maximum integral windup",
00313                                                         "Config: MSB=Deadband. LSB=Sign",
00314                                                         "Config: CRC",
00315                                                         };
00316 #endif
00317 
00318 
00325 typedef enum
00326 {
00327     MOTOR_DEBUG_INVALID                 = 0x0,                  
00328     MOTOR_DEBUG_TORQUE_DEMAND           = 0x1,                  
00329     MOTOR_DEBUG_SGL                     = 0x2,                  
00330     MOTOR_DEBUG_SGR                     = 0x3,                  
00331     MOTOR_DEBUG_CURRENT                 = 0x4,                  
00332     MOTOR_DEBUG_TEMPERATURE             = 0x5                   
00333 }TO_MOTOR_DEBUG_TYPE;
00334 
00335 
00336 
00339 typedef struct
00340 {
00341     int16s              torque;                                 
00342     int16u              misc;                                   
00343 }MOTOR_DATA_PACKET;
00344 
00345 
00346 
00347 #define MESSAGE_ID_MOTOR_ID_SHIFT_POS   5                       //!< Bit number of lowest bit of MESSAGE_ID_MOTOR_ID_BITS
00348 #define MESSAGE_ID_MOTOR_ID_BOTTOM_BIT  0b00000100000           //!< Used to mask for even/odd motors
00349 #define MESSAGE_ID_MOTOR_ID_TOP_2_BITS  0b00110000000           //!< Used to group motors into fours.
00350 
00351 
00352 
00353 #define SENSORS_NUM_0220  ((int)36)                             //!< The number of sensors in the robot.
00354 
00355 
00356 #define JOINTS_NUM_0220   ((int)28)                             //!< The number of joints in the hand
00357 
00358 
00359 
00360 //#if (int)IGNORE > SENSORS_NUM
00361 //    #error Not enough sensors[] in ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS
00362 //#endif
00363 
00365 typedef struct
00366 {
00367     EDC_COMMAND                 EDC_command;                        
00368 
00369 
00370 
00371 
00372         int16u                                      sensors[SENSORS_NUM_0220+1];
00373 
00374     FROM_MOTOR_DATA_TYPE        motor_data_type;                    
00375 
00376 
00377     int16s                      which_motors;                       
00378 
00379 
00380 
00381     int32u                      which_motor_data_arrived;           
00382     int32u                      which_motor_data_had_errors;        
00383 
00384     MOTOR_DATA_PACKET           motor_data_packet[10];              
00385 
00386     int32u                      tactile_data_type;
00387     int16u                      tactile_data_valid;                 
00388     TACTILE_SENSOR_STATUS_v1    tactile[5];                         //
00389 
00390     int16u                      idle_time_us;                       
00391 
00392 
00393 } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS;
00394 
00395 
00396 
00398 typedef struct
00399 {
00400     EDC_COMMAND                 EDC_command;                        
00401 
00402     FROM_MOTOR_DATA_TYPE        from_motor_data_type;               
00403     int16s                      which_motors;                       
00404 
00405 
00406     TO_MOTOR_DATA_TYPE          to_motor_data_type;
00407     int16s                      motor_data[NUM_MOTORS];             
00408 
00409     int32u                      tactile_data_type;                  
00410 
00411 } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND;
00412 
00413 /*
00414 #define ETHERCAT_COMMAND_HEADER_SIZE  (  sizeof(EDC_COMMAND)           \       //!< What's the minimum amount of
00415                                        + sizeof(FROM_MOTOR_DATA_TYPE)   \
00416                                        + sizeof(int16s)  )
00417 */
00418 
00419 #define PALM_0200_ETHERCAT_COMMAND_HEADER_SIZE  (  sizeof(EDC_COMMAND) + sizeof(FROM_MOTOR_DATA_TYPE) + sizeof(int16s)  )
00420 
00421 #define PALM_0200_ETHERCAT_STATUS_DATA_SIZE       sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS)
00422 #define PALM_0200_ETHERCAT_COMMAND_DATA_SIZE      sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND)
00423 
00424                                                 //  Now we need to be *sure* that the Host and the Slave definitely
00425                                                 //  agree on the size of the EtherCAT packets, even if the host is a
00426                                                 //  64-bit machine or something. So we have these calculated sizes.
00427                                                 //  The host and slave can ASSERT that the sizeof() the packets
00428                                                 //  matches the agreed sizes.
00429 #define ETHERCAT_STATUS_AGREED_SIZE     232     //! This is the size of the Status  EtherCAT packet (Status + CAN packet)
00430 #define ETHERCAT_COMMAND_AGREED_SIZE    70      //! This is the size of the Command EtherCAT packet (Status + CAN packet)
00431 
00432 
00433 
00445 
00446 #define PALM_0200_ETHERCAT_COMMAND_DATA_ADDRESS               0x1000
00447 #define PALM_0200_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS    (PALM_0200_ETHERCAT_COMMAND_DATA_ADDRESS            + PALM_0200_ETHERCAT_COMMAND_DATA_SIZE)
00448 
00449 #define PALM_0200_ETHERCAT_STATUS_DATA_ADDRESS                (PALM_0200_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS + ETHERCAT_CAN_BRIDGE_DATA_SIZE)
00450 #define PALM_0200_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS     (PALM_0200_ETHERCAT_STATUS_DATA_ADDRESS             + PALM_0200_ETHERCAT_STATUS_DATA_SIZE)
00451 
00452 //#define NUM_CONFIGS_REQUIRED 5
00453 
00454 
00455 #endif


sr_external_dependencies
Author(s): Ugo Cupcic
autogenerated on Mon Jul 1 2019 20:06:25