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
00032
00033
00034
00035 #ifndef WG0X_H
00036 #define WG0X_H
00037
00038 #include <ethercat_hardware/ethercat_device.h>
00039 #include <ethercat_hardware/motor_model.h>
00040
00041 #include <realtime_tools/realtime_publisher.h>
00042 #include <pr2_msgs/PressureState.h>
00043 #include <pr2_msgs/AccelerometerState.h>
00044
00045 enum MbxCmdType {LOCAL_BUS_READ=1, LOCAL_BUS_WRITE=2};
00046
00047 struct WG0XMbxHdr
00048 {
00049 uint16_t address_;
00050 union
00051 {
00052 uint16_t command_;
00053 struct
00054 {
00055 uint16_t length_:12;
00056 uint16_t seqnum_: 3;
00057 uint16_t write_nread_:1;
00058 }__attribute__ ((__packed__));
00059 };
00060 uint8_t checksum_;
00061
00062 bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum);
00063 bool verifyChecksum(void) const;
00064 }__attribute__ ((__packed__));
00065
00066 static const unsigned MBX_SIZE = 512;
00067 static const unsigned MBX_DATA_SIZE = (MBX_SIZE - sizeof(WG0XMbxHdr) - 1);
00068 struct WG0XMbxCmd
00069 {
00070 WG0XMbxHdr hdr_;
00071 uint8_t data_[MBX_DATA_SIZE];
00072 uint8_t checksum_;
00073
00074 bool build(unsigned address, unsigned length, MbxCmdType type, unsigned seqnum, void const* data);
00075 }__attribute__ ((__packed__));
00076
00077 struct WG0XSpiEepromCmd
00078 {
00079 uint16_t page_;
00080 union
00081 {
00082 uint8_t command_;
00083 struct
00084 {
00085 uint8_t operation_ :4;
00086 uint8_t start_ :1;
00087 uint8_t busy_ :1;
00088 uint8_t unused2_ :2;
00089 }__attribute__ ((__packed__));
00090 };
00091
00092 void build_read(unsigned page)
00093 {
00094 this->page_ = page & 0xffff;
00095 this->operation_ = SPI_READ_OP;
00096 this->start_ = 1;
00097 }
00098 void build_write(unsigned page)
00099 {
00100 this->page_ = page & 0xffff;
00101 this->operation_ = SPI_WRITE_OP;
00102 this->start_ = 1;
00103 }
00104 void build_arbitrary(unsigned length)
00105 {
00106 this->page_ = (length-1) & 0xffff;
00107 this->operation_ = SPI_ARBITRARY_OP;
00108 this->start_ = 1;
00109 }
00110
00111 static const unsigned SPI_READ_OP = 0;
00112 static const unsigned SPI_WRITE_OP = 1;
00113 static const unsigned SPI_ARBITRARY_OP = 3;
00114
00115 static const unsigned SPI_COMMAND_ADDR = 0x0230;
00116 static const unsigned SPI_BUFFER_ADDR = 0xF400;
00117 }__attribute__ ((__packed__));
00118
00119
00120
00121 struct SyncManControl {
00122 union {
00123 uint8_t raw;
00124 struct {
00125 uint8_t mode : 2;
00126 uint8_t direction : 2;
00127 uint8_t ecat_irq_enable : 1;
00128 uint8_t pdi_irq_enable : 1;
00129 uint8_t watchdog_enable : 1;
00130 uint8_t res1 : 1;
00131 } __attribute__ ((__packed__));
00132 } __attribute__ ((__packed__));
00133
00134
00135
00136 } __attribute__ ((__packed__));
00137
00138
00139 struct SyncManStatus {
00140 union {
00141 uint8_t raw;
00142 struct {
00143 uint8_t interrupt_write : 1;
00144 uint8_t interrupt_read : 1;
00145 uint8_t res1 : 1;
00146 uint8_t mailbox_status : 1;
00147 uint8_t buffer_status : 2;
00148 uint8_t res2 : 2;
00149 } __attribute__ ((__packed__));
00150 } __attribute__ ((__packed__));
00151
00152
00153
00154 } __attribute__ ((__packed__));
00155
00156
00157 struct SyncManActivate {
00158 union {
00159 uint8_t raw;
00160 struct {
00161 uint8_t enable : 1;
00162 uint8_t repeat_request : 1;
00163 uint8_t res4 : 4;
00164 uint8_t ecat_latch_event : 1;
00165 uint8_t pdi_latch_event : 1;
00166 } __attribute__ ((__packed__));
00167 } __attribute__ ((__packed__));
00168 static const unsigned BASE_ADDR=0x806;
00169 static unsigned baseAddress(unsigned num);
00170
00171 bool writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EthercatDevice::AddrMode addrMode, unsigned num) const;
00172 } __attribute__ ((__packed__));
00173
00174
00175 struct SyncManPDIControl {
00176 union {
00177 uint8_t raw;
00178 struct {
00179 uint8_t deactivate : 1;
00180 uint8_t repeat_ack : 1;
00181 uint8_t res6 : 6;
00182 } __attribute__ ((__packed__));
00183 } __attribute__ ((__packed__));
00184
00185
00186
00187 } __attribute__ ((__packed__));
00188
00189
00190
00191 struct SyncMan {
00192 union {
00193 uint8_t raw[8];
00194 struct {
00195 uint16_t start_addr;
00196 uint16_t length;
00197 SyncManControl control;
00198 SyncManStatus status;
00199 SyncManActivate activate;
00200 SyncManPDIControl pdi_control;
00201 } __attribute__ ((__packed__));
00202 } __attribute__ ((__packed__));
00203
00204
00205 static const unsigned BASE_ADDR=0x800;
00206
00207 static unsigned baseAddress(unsigned num);
00208
00209 bool readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, EthercatDevice::AddrMode addrMode, unsigned num);
00210
00211 } __attribute__ ((__packed__));
00212
00213
00214 struct WG0XSafetyDisableStatus
00215 {
00216 uint8_t safety_disable_status_;
00217 uint8_t safety_disable_status_hold_;
00218 uint8_t safety_disable_count_;
00219 static const unsigned BASE_ADDR = 0xA1;
00220 } __attribute__ ((__packed__));
00221
00222
00223 struct WG0XSafetyDisableCounters
00224 {
00225 uint8_t undervoltage_count_;
00226 uint8_t over_current_count_;
00227 uint8_t board_over_temp_count_;
00228 uint8_t bridge_over_temp_count_;
00229 uint8_t operate_disable_count_;
00230 uint8_t watchdog_disable_count_;
00231 static const unsigned BASE_ADDR = 0x223;
00232 } __attribute__ ((__packed__));
00233
00234 struct WG0XDiagnosticsInfo
00235 {
00236 int16_t config_offset_current_A_;
00237 int16_t config_offset_current_B_;
00238 uint16_t supply_current_in_;
00239 union {
00240 uint16_t supply_current_out_;
00241 uint16_t voltage_ref_;
00242 } __attribute__ ((__packed__));
00243 int16_t offset_current_A_;
00244 int16_t offset_current_B_;
00245 int16_t adc_current_;
00246 uint8_t unused1[2];
00247 uint8_t lowside_deadtime_;
00248 uint8_t highside_deadtime_;
00249 uint8_t unused2[14];
00250 uint8_t pdo_command_irq_count_;
00251 uint8_t mbx_command_irq_count_;
00252 uint8_t unused3;
00253 WG0XSafetyDisableCounters safety_disable_counters_;
00254 uint8_t unused4;
00255 uint8_t pdi_timeout_error_count_;
00256 uint8_t pdi_checksum_error_count_;
00257 static const unsigned BASE_ADDR = 0x200;
00258 } __attribute__ ((__packed__));
00259
00260 struct WG0XConfigInfo
00261 {
00262 uint32_t product_id_;
00263 union
00264 {
00265 uint32_t revision_;
00266 struct
00267 {
00268 uint8_t firmware_minor_revision_;
00269 uint8_t firmware_major_revision_;
00270 uint8_t pca_revision_;
00271 uint8_t pcb_revision_;
00272 }__attribute__ ((__packed__));
00273 }__attribute__ ((__packed__));
00274 uint32_t device_serial_number_;
00275 uint8_t current_loop_kp_;
00276 uint8_t current_loop_ki_;
00277 uint16_t absolute_current_limit_;
00278 float nominal_current_scale_;
00279 float nominal_voltage_scale_;
00280 uint8_t pad_[8];
00281 uint8_t configuration_status_;
00282 uint8_t safety_disable_status_;
00283 uint8_t safety_disable_status_hold_;
00284 uint8_t safety_disable_count_;
00285 uint16_t watchdog_limit_;
00286
00287 static const unsigned CONFIG_INFO_BASE_ADDR = 0x0080;
00288 }__attribute__ ((__packed__));
00289
00290 struct WG0XUserConfigRam
00291 {
00292 uint8_t version_;
00293 uint8_t unused_[3];
00294 double zero_offset_;
00295 uint32_t crc32_;
00296
00297 static const unsigned BASE_ADDR = 0x00C0;
00298 }__attribute__ ((__packed__));
00299
00300 struct WG0XActuatorInfo
00301 {
00302 uint16_t major_;
00303 uint16_t minor_;
00304 uint32_t id_;
00305 char name_[64];
00306 char robot_name_[32];
00307 char motor_make_[32];
00308 char motor_model_[32];
00309 double max_current_;
00310 double speed_constant_;
00311 double resistance_;
00312 double motor_torque_constant_;
00313 double encoder_reduction_;
00314 uint32_t pulses_per_revolution_;
00315 uint8_t pad1[40];
00316 uint32_t crc32_256_;
00317 uint8_t pad2[4];
00318 uint32_t crc32_264_;
00319 };
00320
00321 struct WG0XStatus
00322 {
00323 uint8_t mode_;
00324 uint8_t digital_out_;
00325 int16_t programmed_pwm_value_;
00326 int16_t programmed_current_;
00327 int16_t measured_current_;
00328 uint32_t timestamp_;
00329 int32_t encoder_count_;
00330 int32_t encoder_index_pos_;
00331 uint16_t num_encoder_errors_;
00332 uint8_t encoder_status_;
00333 uint8_t calibration_reading_;
00334 int32_t last_calibration_rising_edge_;
00335 int32_t last_calibration_falling_edge_;
00336 uint16_t board_temperature_;
00337 uint16_t bridge_temperature_;
00338 uint16_t supply_voltage_;
00339 int16_t motor_voltage_;
00340 uint16_t packet_count_;
00341 uint8_t pad_;
00342 uint8_t checksum_;
00343 }__attribute__ ((__packed__));
00344
00345 struct WG06StatusWithAccel
00346 {
00347 uint8_t mode_;
00348 uint8_t digital_out_;
00349 int16_t programmed_pwm_value_;
00350 int16_t programmed_current_;
00351 int16_t measured_current_;
00352 uint32_t timestamp_;
00353 int32_t encoder_count_;
00354 int32_t encoder_index_pos_;
00355 uint16_t num_encoder_errors_;
00356 uint8_t encoder_status_;
00357 uint8_t unused1;
00358 int32_t unused2;
00359 int32_t unused3;
00360 uint16_t board_temperature_;
00361 uint16_t bridge_temperature_;
00362 uint16_t supply_voltage_;
00363 int16_t motor_voltage_;
00364 uint16_t packet_count_;
00365 uint8_t pad_;
00366 uint8_t accel_count_;
00367 uint32_t accel_[4];
00368 uint8_t checksum_;
00369 }__attribute__ ((__packed__));
00370
00371 struct WG021Status
00372 {
00373 uint8_t mode_;
00374 uint8_t digital_out_;
00375 uint8_t general_config_;
00376 uint8_t pad1_;
00377 int16_t programmed_current_;
00378 int16_t measured_current_;
00379 uint32_t timestamp_;
00380 uint8_t config0_;
00381 uint8_t config1_;
00382 uint8_t config2_;
00383 uint8_t pad2_;
00384 uint32_t pad3_;
00385 uint16_t pad4_;
00386 uint8_t pad5_;
00387 uint8_t output_status_;
00388 uint32_t output_start_timestamp_;
00389 uint32_t output_stop_timestamp_;
00390 uint16_t board_temperature_;
00391 uint16_t bridge_temperature_;
00392 uint16_t supply_voltage_;
00393 int16_t led_voltage_;
00394 uint16_t packet_count_;
00395 uint8_t pad_;
00396 uint8_t checksum_;
00397 }__attribute__ ((__packed__));
00398
00399 struct WG0XCommand
00400 {
00401 uint8_t mode_;
00402 uint8_t digital_out_;
00403 int16_t programmed_pwm;
00404 int16_t programmed_current_;
00405 uint8_t pad_;
00406 uint8_t checksum_;
00407 }__attribute__ ((__packed__));
00408
00409 struct WG021Command
00410 {
00411 uint8_t mode_;
00412 uint8_t digital_out_;
00413 uint8_t general_config_;
00414 uint8_t pad1_;
00415 int16_t programmed_current_;
00416 int16_t pad2_;
00417 int32_t pad3_;
00418 uint8_t config0_;
00419 uint8_t config1_;
00420 uint8_t config2_;
00421 uint8_t checksum_;
00422 }__attribute__ ((__packed__));
00423
00424 struct MbxDiagnostics
00425 {
00426 MbxDiagnostics();
00427 uint32_t write_errors_;
00428 uint32_t read_errors_;
00429 uint32_t lock_errors_;
00430 uint32_t retries_;
00431 uint32_t retry_errors_;
00432 };
00433
00434 struct WG0XDiagnostics
00435 {
00436 WG0XDiagnostics();
00437 void update(const WG0XSafetyDisableStatus &new_status, const WG0XDiagnosticsInfo &new_diagnostics_info);
00438
00439 bool first_;
00440 bool valid_;
00441 WG0XSafetyDisableStatus safety_disable_status_;
00442
00443 WG0XDiagnosticsInfo diagnostics_info_;
00444
00445 uint32_t safety_disable_total_;
00446 uint32_t undervoltage_total_;
00447 uint32_t over_current_total_;
00448 uint32_t board_over_temp_total_;
00449 uint32_t bridge_over_temp_total_;
00450 uint32_t operate_disable_total_;
00451 uint32_t watchdog_disable_total_;
00452
00453 uint32_t lock_errors_;
00454 uint32_t checksum_errors_;
00455
00456
00457 double zero_offset_;
00458 double cached_zero_offset_;
00459 };
00460
00461 class WG0X : public EthercatDevice
00462 {
00463 public:
00464 void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00465 WG0X();
00466 virtual ~WG0X();
00467
00468 virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00469
00470 void packCommand(unsigned char *buffer, bool halt, bool reset);
00471 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00472
00473
00474 void program(WG0XActuatorInfo *);
00475
00476 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00477 virtual void collectDiagnostics(EthercatCom *com);
00478
00479 bool publishTrace(const string &reason, unsigned level, unsigned delay);
00480
00481 protected:
00482 uint8_t fw_major_;
00483 uint8_t fw_minor_;
00484 uint8_t board_major_;
00485 uint8_t board_minor_;
00486
00487 WG0XActuatorInfo actuator_info_;
00488 WG0XConfigInfo config_info_;
00489 double max_current_;
00490
00491 pr2_hardware_interface::Actuator actuator_;
00492 pr2_hardware_interface::DigitalOut digital_out_;
00493
00494 enum
00495 {
00496 MODE_OFF = 0x00,
00497 MODE_ENABLE = (1 << 0),
00498 MODE_CURRENT = (1 << 1),
00499 MODE_SAFETY_RESET = (1 << 4),
00500 MODE_SAFETY_LOCKOUT = (1 << 5),
00501 MODE_UNDERVOLTAGE = (1 << 6),
00502 MODE_RESET = (1 << 7)
00503 };
00504
00505 static string modeString(uint8_t mode);
00506 static string safetyDisableString(uint8_t status);
00507 bool in_lockout_;
00508 bool resetting_;
00509 bool has_error_;
00510 uint16_t max_bridge_temperature_, max_board_temperature_;
00511 bool too_many_dropped_packets_;
00512 bool status_checksum_error_;
00513 bool timestamp_jump_detected_;
00514 bool fpga_internal_reset_detected_;
00515
00516 void clearErrorFlags(void);
00517
00518 double cached_zero_offset_;
00519 enum {NO_CALIBRATION=0, CONTROLLER_CALIBRATION=1, SAVED_CALIBRATION=2};
00520 int calibration_status_;
00521 unsigned last_num_encoder_errors_;
00522
00528 ros::Duration sample_timestamp_;
00529
00531
00532
00533
00534
00535
00536
00537 enum AppRamStatus { APP_RAM_PRESENT=1, APP_RAM_MISSING=2, APP_RAM_NOT_APPLICABLE=3 };
00538 AppRamStatus app_ram_status_;
00539 bool readAppRam(EthercatCom *com, double &zero_offset);
00540 bool writeAppRam(EthercatCom *com, double zero_offset);
00541
00542 bool verifyState(WG0XStatus *this_status, WG0XStatus *prev_status);
00543 int readEeprom(EthercatCom *com);
00544 int writeEeprom(EthercatCom *com);
00545 int sendSpiCommand(EthercatCom *com, WG0XSpiEepromCmd const * cmd);
00546
00547 int writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length);
00548 int readMailbox(EthercatCom *com, unsigned address, void *data, unsigned length);
00549
00550 void publishGeneralDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00551 void publishMailboxDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00552
00553 bool initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw,
00554 const string &device_description,
00555 double max_pwm_ratio,
00556 double board_resistance,
00557 bool poor_measured_motor_voltage);
00558
00559 bool verifyChecksum(const void* buffer, unsigned size);
00560 static bool timestamp_jump(uint32_t timestamp, uint32_t last_timestamp, uint32_t amount);
00561
00562 static const int PWM_MAX = 0x4000;
00563
00564 private:
00565
00566 bool lockMailbox();
00567 void unlockMailbox();
00568 pthread_mutex_t mailbox_lock_;
00569 MbxDiagnostics mailbox_diagnostics_;
00570 MbxDiagnostics mailbox_publish_diagnostics_;
00571
00572
00573 int writeMailbox_(EthercatCom *com, unsigned address, void const *data, unsigned length);
00574 int readMailbox_(EthercatCom *com, unsigned address, void *data, unsigned length);
00575 bool verifyDeviceStateForMailboxOperation();
00576 bool clearReadMailbox(EthercatCom *com);
00577 bool waitForReadMailboxReady(EthercatCom *com);
00578 bool waitForWriteMailboxReady(EthercatCom *com);
00579 bool readMailboxRepeatRequest(EthercatCom *com);
00580 bool _readMailboxRepeatRequest(EthercatCom *com);
00581 bool writeMailboxInternal(EthercatCom *com, void const *data, unsigned length);
00582 bool readMailboxInternal(EthercatCom *com, void *data, unsigned length);
00583 void diagnoseMailboxError(EthercatCom *com);
00584
00585 static const unsigned COMMAND_PHY_ADDR = 0x1000;
00586 static const unsigned STATUS_PHY_ADDR = 0x2000;
00587 static const unsigned PRESSURE_PHY_ADDR = 0x2200;
00588 static const unsigned MBX_COMMAND_PHY_ADDR = 0x1400;
00589 static const unsigned MBX_COMMAND_SIZE = 512;
00590 static const unsigned MBX_STATUS_PHY_ADDR = 0x2400;
00591 static const unsigned MBX_STATUS_SIZE = 512;
00592
00593 static const unsigned PDO_COMMAND_SYNCMAN_NUM = 0;
00594 static const unsigned PDO_STATUS_SYNCMAN_NUM = 1;
00595 static const unsigned MBX_COMMAND_SYNCMAN_NUM = 2;
00596 static const unsigned MBX_STATUS_SYNCMAN_NUM = 3;
00597
00598 enum
00599 {
00600 LIMIT_SENSOR_0_STATE = (1 << 0),
00601 LIMIT_SENSOR_1_STATE = (1 << 1),
00602 LIMIT_ON_TO_OFF = (1 << 2),
00603 LIMIT_OFF_TO_ON = (1 << 3)
00604 };
00605
00606 enum
00607 {
00608 SAFETY_DISABLED = (1 << 0),
00609 SAFETY_UNDERVOLTAGE = (1 << 1),
00610 SAFETY_OVER_CURRENT = (1 << 2),
00611 SAFETY_BOARD_OVER_TEMP = (1 << 3),
00612 SAFETY_HBRIDGE_OVER_TEMP = (1 << 4),
00613 SAFETY_OPERATIONAL = (1 << 5),
00614 SAFETY_WATCHDOG = (1 << 6)
00615 };
00616
00617
00618
00619 static const int ACTUATOR_INFO_PAGE = 4095;
00620
00621
00622
00623 MotorModel *motor_model_;
00624 bool disable_motor_model_checking_;
00625 ethercat_hardware::MotorTraceSample motor_trace_sample_;
00626 pr2_hardware_interface::DigitalOut publish_motor_trace_;
00627
00628
00629 uint32_t last_timestamp_;
00630 uint32_t last_last_timestamp_;
00631 int drops_;
00632 int consecutive_drops_;
00633 int max_consecutive_drops_;
00634
00635 bool lockWG0XDiagnostics();
00636 bool tryLockWG0XDiagnostics();
00637 void unlockWG0XDiagnostics();
00638 pthread_mutex_t wg0x_diagnostics_lock_;
00639 WG0XDiagnostics wg0x_publish_diagnostics_;
00640 WG0XDiagnostics wg0x_collect_diagnostics_;
00641
00642 public:
00643 static int32_t timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp);
00644 static int32_t positionDiff(int32_t new_position, int32_t old_position);
00645
00646 static ros::Duration timediffToDuration(int32_t timediff_usec);
00647
00648 static double calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp,
00649 int32_t old_position, uint32_t old_timestamp);
00650 };
00651
00652 class WG05 : public WG0X
00653 {
00654 public:
00655 int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00656 void packCommand(unsigned char *buffer, bool halt, bool reset);
00657 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00658 enum
00659 {
00660 PRODUCT_CODE = 6805005
00661 };
00662 };
00663
00664 struct WG06Pressure
00665 {
00666 uint32_t timestamp_;
00667 uint16_t l_finger_tip_[22];
00668 uint16_t r_finger_tip_[22];
00669 uint8_t pad_;
00670 uint8_t checksum_;
00671 } __attribute__((__packed__));
00672
00673 class WG06 : public WG0X
00674 {
00675 public:
00676 WG06();
00677 ~WG06();
00678 int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00679 void packCommand(unsigned char *buffer, bool halt, bool reset);
00680 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00681 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00682 enum
00683 {
00684 PRODUCT_CODE = 6805006
00685 };
00686 private:
00687 pr2_hardware_interface::PressureSensor pressure_sensors_[2];
00688 pr2_hardware_interface::Accelerometer accelerometer_;
00689
00690 bool pressure_checksum_error_;
00691
00692 unsigned accelerometer_samples_;
00693 unsigned accelerometer_missed_samples_;
00694 ros::Time last_publish_time_;
00695 bool first_publish_;
00696
00697 uint32_t last_pressure_time_;
00698 realtime_tools::RealtimePublisher<pr2_msgs::PressureState> *pressure_publisher_;
00699 realtime_tools::RealtimePublisher<pr2_msgs::AccelerometerState> *accel_publisher_;
00700 };
00701
00702 class WG021 : public WG0X
00703 {
00704 public:
00705 WG021() : projector_(digital_out_A_, digital_out_B_, digital_out_I_, digital_out_M_, digital_out_L0_, digital_out_L1_) {}
00706 void construct(EtherCAT_SlaveHandler *sh, int &start_address) {WG0X::construct(sh, start_address);}
00707 int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00708 void packCommand(unsigned char *buffer, bool halt, bool reset);
00709 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00710 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00711 enum
00712 {
00713 PRODUCT_CODE = 6805021
00714 };
00715 enum
00716 {
00717 PROJECTOR_CONFIG_ENABLE = 8,
00718 PROJECTOR_CONFIG_ENABLE_ENABLED = 8,
00719 PROJECTOR_CONFIG_ENABLE_DISABLED = 0,
00720
00721 PROJECTOR_CONFIG_ACTION = 4,
00722 PROJECTOR_CONFIG_ACTION_ON = 4,
00723 PROJECTOR_CONFIG_ACTION_OFF = 0,
00724
00725 PROJECTOR_CONFIG_POLARITY = 2,
00726 PROJECTOR_CONFIG_POLARITY_ACTIVE_HIGH = 2,
00727 PROJECTOR_CONFIG_POLARITY_ACTIVE_LOW = 0,
00728
00729 PROJECTOR_CONFIG_STATE = 1,
00730 PROJECTOR_CONFIG_STATE_HIGH = 1,
00731 PROJECTOR_CONFIG_STATE_LOW = 0
00732 };
00733 private:
00734 pr2_hardware_interface::DigitalOut digital_out_A_;
00735 pr2_hardware_interface::DigitalOut digital_out_B_;
00736 pr2_hardware_interface::DigitalOut digital_out_I_;
00737 pr2_hardware_interface::DigitalOut digital_out_M_;
00738 pr2_hardware_interface::DigitalOut digital_out_L0_;
00739 pr2_hardware_interface::DigitalOut digital_out_L1_;
00740 pr2_hardware_interface::Projector projector_;
00741 };
00742
00743 #endif