wg0x.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef ETHERCAT_HARDWARE__WG0X_H
00036 #define ETHERCAT_HARDWARE__WG0X_H
00037 
00038 #include "ethercat_hardware/ethercat_device.h"
00039 #include "ethercat_hardware/motor_model.h"
00040 #include "ethercat_hardware/motor_heating_model.h"
00041 #include "realtime_tools/realtime_publisher.h"
00042 #include "ethercat_hardware/wg_mailbox.h"
00043 #include "ethercat_hardware/wg_eeprom.h"
00044 
00045 #include <boost/shared_ptr.hpp>
00046 
00047 using namespace ethercat_hardware;
00048 
00049 
00050 struct WG0XSafetyDisableStatus
00051 {
00052   uint8_t safety_disable_status_;
00053   uint8_t safety_disable_status_hold_;
00054   uint8_t safety_disable_count_;
00055   static const unsigned BASE_ADDR = 0xA1;
00056 } __attribute__ ((__packed__));
00057 
00058 
00059 struct WG0XSafetyDisableCounters
00060 {
00061   uint8_t undervoltage_count_;
00062   uint8_t over_current_count_;
00063   uint8_t board_over_temp_count_;
00064   uint8_t bridge_over_temp_count_;
00065   uint8_t operate_disable_count_;
00066   uint8_t watchdog_disable_count_;
00067   static const unsigned BASE_ADDR = 0x223;
00068 } __attribute__ ((__packed__));
00069 
00070 struct WG0XDiagnosticsInfo
00071 {
00072   int16_t config_offset_current_A_;
00073   int16_t config_offset_current_B_;
00074   uint16_t supply_current_in_;
00075   union {
00076     uint16_t supply_current_out_;
00077     uint16_t voltage_ref_;
00078   } __attribute__ ((__packed__));
00079   int16_t offset_current_A_;
00080   int16_t offset_current_B_;
00081   int16_t adc_current_;
00082   uint8_t unused1[2];
00083   uint8_t lowside_deadtime_;
00084   uint8_t highside_deadtime_;
00085   uint8_t unused2[14];
00086   uint8_t pdo_command_irq_count_;
00087   uint8_t mbx_command_irq_count_;
00088   uint8_t unused3;
00089   WG0XSafetyDisableCounters safety_disable_counters_;
00090   uint8_t unused4;
00091   uint8_t pdi_timeout_error_count_;
00092   uint8_t pdi_checksum_error_count_;
00093   static const unsigned BASE_ADDR = 0x200;
00094 } __attribute__ ((__packed__));
00095 
00096 struct WG0XConfigInfo
00097 {
00098   uint32_t product_id_;
00099   union
00100   {
00101     uint32_t revision_;
00102     struct
00103     {
00104       uint8_t firmware_minor_revision_;
00105       uint8_t firmware_major_revision_;
00106       uint8_t pca_revision_;
00107       uint8_t pcb_revision_;
00108     }__attribute__ ((__packed__));
00109   }__attribute__ ((__packed__));
00110   uint32_t device_serial_number_;
00111   uint8_t current_loop_kp_;
00112   uint8_t current_loop_ki_;
00113   uint16_t absolute_current_limit_;
00114   float nominal_current_scale_;
00115   float nominal_voltage_scale_;
00116   uint8_t pad_[8];
00117   uint8_t configuration_status_;
00118   uint8_t safety_disable_status_;
00119   uint8_t safety_disable_status_hold_;
00120   uint8_t safety_disable_count_;
00121   uint16_t watchdog_limit_;
00122 
00123   static const unsigned CONFIG_INFO_BASE_ADDR = 0x0080;
00124 }__attribute__ ((__packed__));
00125 
00126 struct WG0XUserConfigRam
00127 {
00128   uint8_t version_;
00129   uint8_t unused_[3];
00130   double zero_offset_;
00131   uint32_t crc32_;
00132 
00133   static const unsigned BASE_ADDR = 0x00C0;
00134 }__attribute__ ((__packed__));
00135 
00136 struct WG0XActuatorInfo
00137 {
00138   uint16_t major_;              // Major revision
00139   uint16_t minor_;              // Minor revision
00140   uint32_t id_;                 // Actuator ID
00141   char name_[64];               // Actuator name
00142   char robot_name_[32];         // Robot name
00143   char motor_make_[32];         // Motor manufacturer
00144   char motor_model_[32];        // Motor model #
00145   double max_current_;          // Maximum current
00146   double speed_constant_;       // Speed constant
00147   double resistance_;           // Resistance
00148   double motor_torque_constant_; // Motor torque constant
00149   double encoder_reduction_;    // Reduction and sign between motor and encoder
00150   uint32_t pulses_per_revolution_; // # of encoder ticks per revolution
00151   uint8_t pad1[40];              // Pad structure to 256-4 bytes.  
00152   uint32_t crc32_256_;          // CRC32 of first 256-4 bytes. (minus 4 bytes for first CRC)
00153   uint8_t pad2[4];              // Pad structure to 264-4 bytes
00154   uint32_t crc32_264_;          // CRC32 over entire structure (minus 4 bytes for second CRC)
00155 
00156   bool verifyCRC(void) const;
00157   void generateCRC(void);
00158 };
00159 
00160 struct WG0XStatus
00161 {
00162   uint8_t mode_;
00163   uint8_t digital_out_;
00164   int16_t programmed_pwm_value_;
00165   int16_t programmed_current_;
00166   int16_t measured_current_;
00167   uint32_t timestamp_;
00168   int32_t encoder_count_;
00169   int32_t encoder_index_pos_;
00170   uint16_t num_encoder_errors_;
00171   uint8_t encoder_status_;
00172   uint8_t calibration_reading_;
00173   int32_t last_calibration_rising_edge_;
00174   int32_t last_calibration_falling_edge_;
00175   uint16_t board_temperature_;
00176   uint16_t bridge_temperature_;
00177   uint16_t supply_voltage_;
00178   int16_t motor_voltage_;
00179   uint16_t packet_count_;
00180   uint8_t pad_;
00181   uint8_t checksum_;
00182 
00183   static const unsigned SIZE=44;
00184 }__attribute__ ((__packed__));
00185 
00186 
00187 struct WG0XCommand
00188 {
00189   uint8_t mode_;
00190   uint8_t digital_out_;
00191   int16_t programmed_pwm;
00192   int16_t programmed_current_;
00193   uint8_t pad_;
00194   uint8_t checksum_;
00195 }__attribute__ ((__packed__));
00196 
00197 struct MbxDiagnostics 
00198 {
00199   MbxDiagnostics();
00200   uint32_t write_errors_;
00201   uint32_t read_errors_;
00202   uint32_t lock_errors_;
00203   uint32_t retries_;
00204   uint32_t retry_errors_;
00205 };
00206 
00207 struct WG0XDiagnostics 
00208 {
00209   WG0XDiagnostics();
00210   void update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info);
00211 
00212   bool first_;
00213   bool valid_;
00214   WG0XSafetyDisableStatus safety_disable_status_;
00215 
00216   WG0XDiagnosticsInfo diagnostics_info_;
00217   
00218   uint32_t safety_disable_total_;
00219   uint32_t undervoltage_total_;
00220   uint32_t over_current_total_;
00221   uint32_t board_over_temp_total_;
00222   uint32_t bridge_over_temp_total_;
00223   uint32_t operate_disable_total_;
00224   uint32_t watchdog_disable_total_;
00225 
00226   uint32_t lock_errors_;
00227   uint32_t checksum_errors_;
00228 
00229   // Hack, use diagnostic thread to push new offset values to device
00230   double zero_offset_;
00231   double cached_zero_offset_;
00232 };
00233 
00234 class WG0X : public EthercatDevice
00235 {
00236 public:
00237   void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00238   WG0X();
00239   virtual ~WG0X();
00240 
00241   virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00242 
00243   void packCommand(unsigned char *buffer, bool halt, bool reset);
00244   bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00245 
00246   bool program(EthercatCom *com, const WG0XActuatorInfo &actutor_info);
00247   bool program(EthercatCom *com, const MotorHeatingModelParametersEepromConfig &heating_config);
00248 
00249   bool readActuatorInfoFromEeprom(EthercatCom *com, WG0XActuatorInfo &actuator_info);
00250   bool readMotorHeatingModelParametersFromEeprom(EthercatCom *com, MotorHeatingModelParametersEepromConfig &config);
00251 
00252   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00253   virtual void collectDiagnostics(EthercatCom *com);
00254 
00255   bool publishTrace(const string &reason, unsigned level, unsigned delay);
00256 
00257 protected:
00258   uint8_t fw_major_;
00259   uint8_t fw_minor_;
00260   uint8_t board_major_;  
00261   uint8_t board_minor_;  
00262 
00263   WG0XActuatorInfo actuator_info_;
00264   WG0XConfigInfo config_info_;
00265   double max_current_;         
00266 
00267   ethercat_hardware::ActuatorInfo actuator_info_msg_; 
00268   static void copyActuatorInfo(ethercat_hardware::ActuatorInfo &out,  const WG0XActuatorInfo &in);
00269 
00270   pr2_hardware_interface::Actuator actuator_;
00271   pr2_hardware_interface::DigitalOut digital_out_;
00272 
00273   enum
00274   {
00275     MODE_OFF = 0x00,
00276     MODE_ENABLE = (1 << 0),
00277     MODE_CURRENT = (1 << 1),
00278     MODE_SAFETY_RESET = (1 << 4),
00279     MODE_SAFETY_LOCKOUT = (1 << 5),
00280     MODE_UNDERVOLTAGE = (1 << 6),
00281     MODE_RESET = (1 << 7)
00282   };
00283 
00284   enum 
00285   {
00286     WG05_PRODUCT_CODE = 6805005,
00287     WG06_PRODUCT_CODE = 6805006,
00288     WG021_PRODUCT_CODE = 6805021
00289   };
00290 
00291   static string modeString(uint8_t mode);
00292   static string safetyDisableString(uint8_t status);
00293   bool in_lockout_;
00294   bool resetting_;
00295   bool has_error_;
00296   uint16_t max_bridge_temperature_, max_board_temperature_;
00297   bool too_many_dropped_packets_;
00298   bool status_checksum_error_;
00299   bool timestamp_jump_detected_;
00300   bool fpga_internal_reset_detected_;
00301   bool encoder_errors_detected_;
00302 
00303   void clearErrorFlags(void);
00304 
00305   double cached_zero_offset_;
00306   enum {NO_CALIBRATION=0, CONTROLLER_CALIBRATION=1, SAVED_CALIBRATION=2};
00307   int calibration_status_;
00308 
00314   ros::Duration sample_timestamp_;
00315   
00317   //  Application ram is non-volitile memory that application can use to store temporary
00318   //  data between runs.
00319   //  Currently app ram is used to store zero offset of joint after calibration
00320   //    PRESENT - App ram is available
00321   //    MISSING - App ram is missing but would typically be availble
00322   //    NOT_APPLICABLE - App ram is not availble, but is is not needed for this type of device 
00323   enum AppRamStatus { APP_RAM_PRESENT=1, APP_RAM_MISSING=2, APP_RAM_NOT_APPLICABLE=3 };
00324   AppRamStatus app_ram_status_; 
00325   bool readAppRam(EthercatCom *com, double &zero_offset);
00326   bool writeAppRam(EthercatCom *com, double zero_offset);
00327 
00328   bool verifyState(WG0XStatus *this_status, WG0XStatus *prev_status);
00329 
00330   int writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length);
00331   int readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length);  
00332 
00333   void publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00334   void publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00335 
00336   bool initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw, 
00337                             const string &device_description,
00338                             double max_pwm_ratio, 
00339                             double board_resistance,
00340                             bool poor_measured_motor_voltage);
00341 
00342   bool initializeMotorHeatingModel(bool allow_unprogrammed);
00343 
00344   bool verifyChecksum(const void* buffer, unsigned size);
00345   static bool timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount);
00346   
00347   static const int PWM_MAX = 0x4000;
00348   
00349 protected:
00351   ethercat_hardware::WGMailbox mailbox_;
00352 
00354   ethercat_hardware::WGEeprom eeprom_;
00355 
00356   static const unsigned COMMAND_PHY_ADDR = 0x1000;
00357   static const unsigned STATUS_PHY_ADDR = 0x2000;
00358 
00359   static const unsigned PDO_COMMAND_SYNCMAN_NUM = 0;
00360   static const unsigned PDO_STATUS_SYNCMAN_NUM  = 1;
00361 
00362   enum
00363   {
00364     LIMIT_SENSOR_0_STATE = (1 << 0),
00365     LIMIT_SENSOR_1_STATE = (1 << 1),
00366     LIMIT_ON_TO_OFF = (1 << 2),
00367     LIMIT_OFF_TO_ON = (1 << 3)
00368   };
00369 
00370   enum
00371   {
00372     SAFETY_DISABLED = (1 << 0),
00373     SAFETY_UNDERVOLTAGE = (1 << 1),
00374     SAFETY_OVER_CURRENT = (1 << 2),
00375     SAFETY_BOARD_OVER_TEMP = (1 << 3),
00376     SAFETY_HBRIDGE_OVER_TEMP = (1 << 4),
00377     SAFETY_OPERATIONAL = (1 << 5),
00378     SAFETY_WATCHDOG = (1 << 6)
00379   };
00380 
00381   // Board configuration parameters
00382 
00383   static const unsigned ACTUATOR_INFO_PAGE = 4095;
00384 
00385   // Not all devices will need this (WG021 won't) 
00386   MotorModel *motor_model_; 
00387   bool disable_motor_model_checking_;
00388   ethercat_hardware::MotorTraceSample motor_trace_sample_;
00389   pr2_hardware_interface::DigitalOut publish_motor_trace_; 
00390 
00391   // Only device with motor heating parameters store in eeprom config will use this
00392   static boost::shared_ptr<ethercat_hardware::MotorHeatingModelCommon> motor_heating_model_common_;
00393   boost::shared_ptr<ethercat_hardware::MotorHeatingModel> motor_heating_model_;
00394 
00395   // Diagnostic message values
00396   uint32_t last_timestamp_;
00397   uint32_t last_last_timestamp_;
00398   int drops_;
00399   int consecutive_drops_;
00400   int max_consecutive_drops_;
00401 
00402   bool lockWG0XDiagnostics();
00403   bool tryLockWG0XDiagnostics();
00404   void unlockWG0XDiagnostics();
00405   pthread_mutex_t wg0x_diagnostics_lock_;
00406   WG0XDiagnostics wg0x_publish_diagnostics_;
00407   WG0XDiagnostics wg0x_collect_diagnostics_;
00408 
00409 public:
00410   static int32_t timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp);
00411   static int32_t positionDiff(int32_t new_position, int32_t old_position);
00412 
00413   static ros::Duration timediffToDuration(int32_t timediff_usec);
00414 
00415   static double calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp, 
00416                                     int32_t old_position, uint32_t old_timestamp);
00417 
00418   static double convertRawTemperature(int16_t raw_temp);
00419 };
00420 
00421 
00422 #endif // ETHERCAT_HARDWARE__WG0X_H


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:45