wg0x.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #ifndef ETHERCAT_HARDWARE__WG0X_H
36 #define ETHERCAT_HARDWARE__WG0X_H
37 
44 
45 #include <boost/shared_ptr.hpp>
46 
47 using namespace ethercat_hardware;
48 
49 
51 {
55  static const unsigned BASE_ADDR = 0xA1;
56 } __attribute__ ((__packed__));
57 
58 
60 {
67  static const unsigned BASE_ADDR = 0x223;
68 } __attribute__ ((__packed__));
69 
71 {
75  union {
77  uint16_t voltage_ref_;
78  } __attribute__ ((__packed__));
81  int16_t adc_current_;
82  uint8_t unused1[2];
85  uint8_t unused2[14];
88  uint8_t unused3;
90  uint8_t unused4;
93  static const unsigned BASE_ADDR = 0x200;
94 } __attribute__ ((__packed__));
95 
97 {
98  uint32_t product_id_;
99  union
100  {
101  uint32_t revision_;
102  struct
103  {
106  uint8_t pca_revision_;
107  uint8_t pcb_revision_;
108  }__attribute__ ((__packed__));
109  }__attribute__ ((__packed__));
116  uint8_t pad_[8];
121  uint16_t watchdog_limit_;
122 
123  static const unsigned CONFIG_INFO_BASE_ADDR = 0x0080;
124 }__attribute__ ((__packed__));
125 
127 {
128  uint8_t version_;
129  uint8_t unused_[3];
130  double zero_offset_;
131  uint32_t crc32_;
132 
133  static const unsigned BASE_ADDR = 0x00C0;
134 }__attribute__ ((__packed__));
135 
137 {
138  uint16_t major_; // Major revision
139  uint16_t minor_; // Minor revision
140  uint32_t id_; // Actuator ID
141  char name_[64]; // Actuator name
142  char robot_name_[32]; // Robot name
143  char motor_make_[32]; // Motor manufacturer
144  char motor_model_[32]; // Motor model #
145  double max_current_; // Maximum current
146  double speed_constant_; // Speed constant
147  double resistance_; // Resistance
148  double motor_torque_constant_; // Motor torque constant
149  double encoder_reduction_; // Reduction and sign between motor and encoder
150  uint32_t pulses_per_revolution_; // # of encoder ticks per revolution
151  uint8_t pad1[40]; // Pad structure to 256-4 bytes.
152  uint32_t crc32_256_; // CRC32 of first 256-4 bytes. (minus 4 bytes for first CRC)
153  uint8_t pad2[4]; // Pad structure to 264-4 bytes
154  uint32_t crc32_264_; // CRC32 over entire structure (minus 4 bytes for second CRC)
155 
156  bool verifyCRC(void) const;
157  void generateCRC(void);
158 };
159 
161 {
162  uint8_t mode_;
163  uint8_t digital_out_;
167  uint32_t timestamp_;
168  int32_t encoder_count_;
177  uint16_t supply_voltage_;
178  int16_t motor_voltage_;
179  uint16_t packet_count_;
180  uint8_t pad_;
181  uint8_t checksum_;
182 
183  static const unsigned SIZE=44;
184 }__attribute__ ((__packed__));
185 
186 
188 {
189  uint8_t mode_;
190  uint8_t digital_out_;
191  int16_t programmed_pwm;
193  uint8_t pad_;
194  uint8_t checksum_;
195 }__attribute__ ((__packed__));
196 
198 {
199  MbxDiagnostics();
200  uint32_t write_errors_;
201  uint32_t read_errors_;
202  uint32_t lock_errors_;
203  uint32_t retries_;
204  uint32_t retry_errors_;
205 };
206 
208 {
209  WG0XDiagnostics();
210  void update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info);
211 
212  bool first_;
213  bool valid_;
215 
217 
225 
226  uint32_t lock_errors_;
228 
229  // Hack, use diagnostic thread to push new offset values to device
230  double zero_offset_;
232 };
233 
234 class WG0X : public EthercatDevice
235 {
236 public:
237  void construct(EtherCAT_SlaveHandler *sh, int &start_address);
238  WG0X();
239  virtual ~WG0X();
240 
241  virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
242 
243  void packCommand(unsigned char *buffer, bool halt, bool reset);
244  bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
245 
246  bool program(EthercatCom *com, const WG0XActuatorInfo &actutor_info);
247  bool program(EthercatCom *com, const MotorHeatingModelParametersEepromConfig &heating_config);
248 
249  bool readActuatorInfoFromEeprom(EthercatCom *com, WG0XActuatorInfo &actuator_info);
250  bool readMotorHeatingModelParametersFromEeprom(EthercatCom *com, MotorHeatingModelParametersEepromConfig &config);
251 
252  void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
253  virtual void collectDiagnostics(EthercatCom *com);
255  bool publishTrace(const string &reason, unsigned level, unsigned delay);
257 protected:
258  uint8_t fw_major_;
259  uint8_t fw_minor_;
260  uint8_t board_major_;
261  uint8_t board_minor_;
263  WG0XActuatorInfo actuator_info_;
264  WG0XConfigInfo config_info_;
265  double max_current_;
267  ethercat_hardware::ActuatorInfo actuator_info_msg_;
268  static void copyActuatorInfo(ethercat_hardware::ActuatorInfo &out, const WG0XActuatorInfo &in);
273  enum
274  {
275  MODE_OFF = 0x00,
276  MODE_ENABLE = (1 << 0),
277  MODE_CURRENT = (1 << 1),
278  MODE_SAFETY_RESET = (1 << 4),
279  MODE_SAFETY_LOCKOUT = (1 << 5),
280  MODE_UNDERVOLTAGE = (1 << 6),
281  MODE_RESET = (1 << 7)
282  };
283 
284  enum
285  {
286  WG05_PRODUCT_CODE = 6805005,
287  WG06_PRODUCT_CODE = 6805006,
288  WG021_PRODUCT_CODE = 6805021
289  };
290 
291  static string modeString(uint8_t mode);
292  static string safetyDisableString(uint8_t status);
296  uint16_t max_bridge_temperature_, max_board_temperature_;
302 
303  void clearErrorFlags(void);
304 
306  enum {NO_CALIBRATION=0, CONTROLLER_CALIBRATION=1, SAVED_CALIBRATION=2};
308 
315 
317  // Application ram is non-volitile memory that application can use to store temporary
318  // data between runs.
319  // Currently app ram is used to store zero offset of joint after calibration
320  // PRESENT - App ram is available
321  // MISSING - App ram is missing but would typically be availble
322  // NOT_APPLICABLE - App ram is not availble, but is is not needed for this type of device
323  enum AppRamStatus { APP_RAM_PRESENT=1, APP_RAM_MISSING=2, APP_RAM_NOT_APPLICABLE=3 };
325  bool readAppRam(EthercatCom *com, double &zero_offset);
326  bool writeAppRam(EthercatCom *com, double zero_offset);
327 
328  bool verifyState(WG0XStatus *this_status, WG0XStatus *prev_status);
329 
330  int writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length);
331  int readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length);
332 
333  void publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
334  void publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
335 
336  bool initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw,
337  const string &device_description,
338  double max_pwm_ratio,
339  double board_resistance,
340  bool poor_measured_motor_voltage);
341 
342  bool initializeMotorHeatingModel(bool allow_unprogrammed);
343 
344  bool verifyChecksum(const void* buffer, unsigned size);
345  static bool timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount);
346 
347  static const int PWM_MAX = 0x4000;
348 
349 protected:
352 
355 
356  static const unsigned COMMAND_PHY_ADDR = 0x1000;
357  static const unsigned STATUS_PHY_ADDR = 0x2000;
358 
359  static const unsigned PDO_COMMAND_SYNCMAN_NUM = 0;
360  static const unsigned PDO_STATUS_SYNCMAN_NUM = 1;
361 
362  enum
363  {
364  LIMIT_SENSOR_0_STATE = (1 << 0),
365  LIMIT_SENSOR_1_STATE = (1 << 1),
366  LIMIT_ON_TO_OFF = (1 << 2),
367  LIMIT_OFF_TO_ON = (1 << 3)
368  };
369 
370  enum
371  {
372  SAFETY_DISABLED = (1 << 0),
373  SAFETY_UNDERVOLTAGE = (1 << 1),
374  SAFETY_OVER_CURRENT = (1 << 2),
375  SAFETY_BOARD_OVER_TEMP = (1 << 3),
376  SAFETY_HBRIDGE_OVER_TEMP = (1 << 4),
377  SAFETY_OPERATIONAL = (1 << 5),
378  SAFETY_WATCHDOG = (1 << 6)
379  };
380 
381  // Board configuration parameters
382 
383  static const unsigned ACTUATOR_INFO_PAGE = 4095;
384 
385  // Not all devices will need this (WG021 won't)
388  ethercat_hardware::MotorTraceSample motor_trace_sample_;
390 
391  // Only device with motor heating parameters store in eeprom config will use this
394 
395  // Diagnostic message values
396  uint32_t last_timestamp_;
398  int drops_;
401 
402  bool lockWG0XDiagnostics();
403  bool tryLockWG0XDiagnostics();
404  void unlockWG0XDiagnostics();
405  pthread_mutex_t wg0x_diagnostics_lock_;
408 
409 public:
410  static int32_t timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp);
411  static int32_t positionDiff(int32_t new_position, int32_t old_position);
412 
413  static ros::Duration timediffToDuration(int32_t timediff_usec);
414 
415  static double calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp,
416  int32_t old_position, uint32_t old_timestamp);
417 
418  static double convertRawTemperature(int16_t raw_temp);
419 };
420 
421 
422 #endif // ETHERCAT_HARDWARE__WG0X_H
uint16_t max_bridge_temperature_
Definition: wg0x.h:296
uint8_t board_over_temp_count_
Definition: wg0x.h:63
uint16_t supply_voltage_
Definition: wg0x.h:177
uint8_t mbx_command_irq_count_
Definition: wg0x.h:87
int16_t programmed_pwm_value_
Definition: wg0x.h:164
uint32_t pulses_per_revolution_
Definition: wg0x.h:150
uint32_t crc32_256_
Definition: wg0x.h:152
uint8_t unused3
Definition: wg0x.h:88
uint8_t unused1[2]
Definition: wg0x.h:262
uint16_t length
Definition: wg_util.h:119
WG0XDiagnostics wg0x_publish_diagnostics_
Definition: wg0x.h:406
uint8_t firmware_major_revision_
Definition: wg0x.h:103
int consecutive_drops_
Definition: wg0x.h:399
uint32_t bridge_over_temp_total_
Definition: wg0x.h:222
double zero_offset_
Definition: wg0x.h:130
bool resetting_
Definition: wg0x.h:294
bool first_
Definition: wg0x.h:212
uint32_t product_id_
Definition: wg0x.h:98
static const unsigned SIZE
Definition: wg0x.h:273
ROSCONSOLE_DECL void initialize()
uint8_t bridge_over_temp_count_
Definition: wg0x.h:64
double speed_constant_
Definition: wg0x.h:146
int max_consecutive_drops_
Definition: wg0x.h:400
bool verifyChecksum(void) const
uint16_t board_temperature_
Definition: wg0x.h:175
static boost::shared_ptr< ethercat_hardware::MotorHeatingModelCommon > motor_heating_model_common_
Definition: wg0x.h:392
MotorModel * motor_model_
Definition: wg0x.h:386
double max_current_
Definition: wg0x.h:145
uint8_t firmware_minor_revision_
Definition: wg0x.h:102
bool too_many_dropped_packets_
Definition: wg0x.h:297
uint16_t watchdog_limit_
Definition: wg0x.h:121
uint8_t undervoltage_count_
Definition: wg0x.h:61
int32_t encoder_count_
Definition: wg0x.h:168
uint8_t safety_disable_status_
Definition: wg0x.h:52
bool has_error_
Definition: wg0x.h:295
uint8_t safety_disable_status_hold_
Definition: wg0x.h:119
uint32_t timestamp_
Definition: wg0x.h:167
uint8_t safety_disable_status_hold_
Definition: wg0x.h:53
uint16_t bridge_temperature_
Definition: wg0x.h:176
ethercat_hardware::WGEeprom eeprom_
Access to device eeprom.
Definition: wg0x.h:354
AppRamStatus
Different possible states for application ram on device.
Definition: wg0x.h:323
WG0XDiagnosticsInfo diagnostics_info_
Definition: wg0x.h:216
uint8_t safety_disable_status_
Definition: wg0x.h:118
int16_t config_offset_current_B_
Definition: wg0x.h:73
float nominal_voltage_scale_
Definition: wg0x.h:115
uint8_t checksum_
Definition: wg0x.h:181
bool status_checksum_error_
Definition: wg0x.h:298
uint8_t checksum_
Definition: wg0x.h:194
uint32_t safety_disable_total_
Definition: wg0x.h:218
double resistance_
Definition: wg0x.h:147
uint8_t mode
Definition: wg_util.h:48
uint8_t current_loop_kp_
Definition: wg0x.h:111
uint8_t current_loop_ki_
Definition: wg0x.h:112
uint8_t safety_disable_count_
Definition: wg0x.h:120
int16_t offset_current_B_
Definition: wg0x.h:80
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
double motor_torque_constant_
Definition: wg0x.h:148
bool fpga_internal_reset_detected_
Definition: wg0x.h:300
uint8_t version_
Definition: wg0x.h:128
static const unsigned CONFIG_INFO_BASE_ADDR
Definition: wg0x.h:277
double cached_zero_offset_
Definition: wg0x.h:305
uint8_t unused2[14]
Definition: wg0x.h:265
int32_t last_calibration_falling_edge_
Definition: wg0x.h:174
bool timestamp_jump_detected_
Definition: wg0x.h:299
double encoder_reduction_
Definition: wg0x.h:149
int32_t last_calibration_rising_edge_
Definition: wg0x.h:173
uint32_t watchdog_disable_total_
Definition: wg0x.h:224
uint32_t operate_disable_total_
Definition: wg0x.h:223
uint8_t digital_out_
Definition: wg0x.h:253
uint8_t highside_deadtime_
Definition: wg0x.h:84
bool valid_
Definition: wg0x.h:213
int calibration_status_
Definition: wg0x.h:307
int16_t config_offset_current_A_
Definition: wg0x.h:72
pthread_mutex_t wg0x_diagnostics_lock_
Definition: wg0x.h:405
double cached_zero_offset_
Definition: wg0x.h:231
ros::Duration sample_timestamp_
Definition: wg0x.h:314
uint32_t id_
Definition: wg0x.h:140
uint16_t absolute_current_limit_
Definition: wg0x.h:113
uint32_t over_current_total_
Definition: wg0x.h:220
uint32_t checksum_errors_
Definition: wg0x.h:227
uint8_t digital_out_
Definition: wg0x.h:163
uint8_t mode_
Definition: wg0x.h:189
uint32_t crc32_264_
Definition: wg0x.h:154
uint16_t status
int32_t encoder_index_pos_
Definition: wg0x.h:169
int16_t programmed_current_
Definition: wg0x.h:192
uint8_t configuration_status_
Definition: wg0x.h:117
uint8_t operate_disable_count_
Definition: wg0x.h:65
int16_t programmed_current_
Definition: wg0x.h:165
uint16_t packet_count_
Definition: wg0x.h:179
int16_t motor_voltage_
Definition: wg0x.h:178
uint16_t voltage_ref_
Definition: wg0x.h:77
ethercat_hardware::MotorTraceSample motor_trace_sample_
Definition: wg0x.h:388
uint8_t mode_
Definition: wg0x.h:162
uint8_t over_current_count_
Definition: wg0x.h:62
uint32_t read_errors_
Definition: wg0x.h:201
bool encoder_errors_detected_
Definition: wg0x.h:301
uint8_t pdi_checksum_error_count_
Definition: wg0x.h:92
static const unsigned BASE_ADDR
Definition: wg0x.h:255
uint8_t unused4
Definition: wg0x.h:90
uint8_t pad2[204]
uint16_t supply_current_in_
Definition: wg0x.h:74
int drops_
Definition: wg0x.h:398
uint16_t major_
Definition: wg0x.h:138
uint8_t pad_
Definition: wg0x.h:193
uint16_t minor_
Definition: wg0x.h:139
int16_t offset_current_A_
Definition: wg0x.h:79
double zero_offset_
Definition: wg0x.h:230
uint8_t encoder_status_
Definition: wg0x.h:171
uint8_t watchdog_disable_count_
Definition: wg0x.h:66
uint32_t last_last_timestamp_
Definition: wg0x.h:397
uint32_t revision_
Definition: wg0x.h:101
pr2_hardware_interface::DigitalOut publish_motor_trace_
Definition: wg0x.h:389
uint8_t pad_[8]
Definition: wg0x.h:270
uint8_t safety_disable_count_
Definition: wg0x.h:54
uint32_t lock_errors_
Definition: wg0x.h:202
WG0XSafetyDisableStatus safety_disable_status_
Definition: wg0x.h:214
uint8_t pca_revision_
Definition: wg0x.h:106
AppRamStatus app_ram_status_
Definition: wg0x.h:324
int16_t programmed_pwm
Definition: wg0x.h:191
uint8_t pad1[3]
uint32_t lock_errors_
Definition: wg0x.h:226
uint32_t device_serial_number_
Definition: wg0x.h:110
float nominal_current_scale_
Definition: wg0x.h:114
uint8_t pdi_timeout_error_count_
Definition: wg0x.h:91
char * name_
Definition: motorconf.cpp:381
int16_t measured_current_
Definition: wg0x.h:166
uint32_t board_over_temp_total_
Definition: wg0x.h:221
void generateCRC(void)
uint8_t pad_
Definition: wg0x.h:180
uint16_t supply_current_out_
Definition: wg0x.h:75
int16_t adc_current_
Definition: wg0x.h:81
boost::shared_ptr< ethercat_hardware::MotorHeatingModel > motor_heating_model_
Definition: wg0x.h:393
WG0XSafetyDisableCounters safety_disable_counters_
Definition: wg0x.h:89
uint8_t pcb_revision_
Definition: wg0x.h:107
uint32_t undervoltage_total_
Definition: wg0x.h:219
uint32_t retry_errors_
Definition: wg0x.h:204
ethercat_hardware::WGMailbox mailbox_
Mailbox access to device.
Definition: wg0x.h:351
uint8_t digital_out_
Definition: wg0x.h:190
uint8_t pdo_command_irq_count_
Definition: wg0x.h:86
uint8_t unused_[3]
Definition: wg0x.h:253
ethercat_hardware::MotorHeatingModelCommon __attribute__
Definition: wg0x.h:234
uint32_t write_errors_
Definition: wg0x.h:200
bool verifyCRC(void) const
uint32_t retries_
Definition: wg0x.h:203
uint32_t crc32_
Definition: wg0x.h:131
WG0XDiagnostics wg0x_collect_diagnostics_
Definition: wg0x.h:407
uint32_t last_timestamp_
Definition: wg0x.h:396
bool in_lockout_
Definition: wg0x.h:293
uint8_t lowside_deadtime_
Definition: wg0x.h:83
bool disable_motor_model_checking_
Definition: wg0x.h:387
uint16_t num_encoder_errors_
Definition: wg0x.h:170
uint8_t calibration_reading_
Definition: wg0x.h:172


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Tue Mar 28 2023 02:10:20