Go to the documentation of this file.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 ETHERCAT_HARDWARE_WG06_H
00036 #define ETHERCAT_HARDWARE_WG06_H
00037
00038 #include <ethercat_hardware/wg0x.h>
00039
00040 #include <ethercat_hardware/wg_soft_processor.h>
00041
00042 #include <pr2_msgs/PressureState.h>
00043 #include <pr2_msgs/AccelerometerState.h>
00044 #include <ethercat_hardware/RawFTData.h>
00045 #include <geometry_msgs/WrenchStamped.h>
00046
00047 struct WG06StatusWithAccel
00048 {
00049 uint8_t mode_;
00050 uint8_t digital_out_;
00051 int16_t programmed_pwm_value_;
00052 int16_t programmed_current_;
00053 int16_t measured_current_;
00054 uint32_t timestamp_;
00055 int32_t encoder_count_;
00056 int32_t encoder_index_pos_;
00057 uint16_t num_encoder_errors_;
00058 uint8_t encoder_status_;
00059 uint8_t unused1;
00060 int32_t unused2;
00061 int32_t unused3;
00062 uint16_t board_temperature_;
00063 uint16_t bridge_temperature_;
00064 uint16_t supply_voltage_;
00065 int16_t motor_voltage_;
00066 uint16_t packet_count_;
00067 uint8_t pad_;
00068 uint8_t accel_count_;
00069 uint32_t accel_[4];
00070 uint8_t checksum_;
00071
00072 static const unsigned SIZE=61;
00073 }__attribute__ ((__packed__));
00074
00075
00076 struct FTDataSample
00077 {
00078 int16_t data_[6];
00079 uint16_t vhalf_;
00080 uint8_t sample_count_;
00081 uint8_t timestamp_;
00082 static const unsigned SIZE=16;
00083 }__attribute__ ((__packed__));
00084
00085
00086 class FTParamsInternal
00087 {
00088 public:
00089 FTParamsInternal();
00090
00091 const double &calibration_coeff(unsigned row, unsigned col) const {return calibration_coeff_[row*6 + col];}
00092 double &calibration_coeff(unsigned row, unsigned col) {return calibration_coeff_[row*6 + col];}
00093
00094 const double &offset(unsigned ch_num) const {return offsets_[ch_num];}
00095 double &offset(unsigned ch_num) {return offsets_[ch_num];}
00096
00097 const double &gain(unsigned ch_num) const {return gains_[ch_num];}
00098 double &gain(unsigned ch_num) {return gains_[ch_num];}
00099
00100 void print() const;
00101
00102 bool getRosParams(ros::NodeHandle nh);
00103 bool getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len);
00104 double calibration_coeff_[36];
00105 double offsets_[6];
00106 double gains_[6];
00107 };
00108
00109
00110 struct WG06StatusWithAccelAndFT
00111 {
00112 uint8_t mode_;
00113 uint8_t digital_out_;
00114 int16_t programmed_pwm_value_;
00115 int16_t programmed_current_;
00116 int16_t measured_current_;
00117 uint32_t timestamp_;
00118 int32_t encoder_count_;
00119 int32_t encoder_index_pos_;
00120 uint16_t num_encoder_errors_;
00121 uint8_t encoder_status_;
00122 uint8_t unused1;
00123 int32_t unused2;
00124 int32_t unused3;
00125 uint16_t board_temperature_;
00126 uint16_t bridge_temperature_;
00127 uint16_t supply_voltage_;
00128 int16_t motor_voltage_;
00129 uint16_t packet_count_;
00130 uint8_t pad_;
00131 uint8_t accel_count_;
00132 uint32_t accel_[4];
00133 uint8_t unused4[3];
00134 uint8_t ft_sample_count_;
00135 FTDataSample ft_samples_[4];
00136 uint8_t checksum_;
00137
00138 static const unsigned SIZE=129;
00139 }__attribute__ ((__packed__));
00140
00141
00142 struct WG06Pressure
00143 {
00144 uint32_t timestamp_;
00145 uint16_t l_finger_tip_[22];
00146 uint16_t r_finger_tip_[22];
00147 uint8_t pad_;
00148 uint8_t checksum_;
00149 static const unsigned SIZE=94;
00150 } __attribute__((__packed__));
00151
00152
00153 struct WG06BigPressure
00154 {
00155 WG06Pressure pressure_;
00156 uint8_t pad_[418];
00157 uint8_t checksum_;
00158 static const unsigned SIZE=513;
00159 } __attribute__((__packed__));
00160
00161
00162
00163 class WG06 : public WG0X
00164 {
00165 public:
00166 WG06();
00167 ~WG06();
00168 int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00169 void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00170 void packCommand(unsigned char *buffer, bool halt, bool reset);
00171 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00172
00173 virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
00174 enum
00175 {
00176 PRODUCT_CODE = 6805006
00177 };
00178 private:
00179
00180 static const unsigned PRESSURE_PHY_ADDR = 0x2200;
00181 static const unsigned BIG_PRESSURE_PHY_ADDR = 0x2600;
00182
00183 pr2_hardware_interface::PressureSensor pressure_sensors_[2];
00184 pr2_hardware_interface::Accelerometer accelerometer_;
00185
00186 bool initializePressure(pr2_hardware_interface::HardwareInterface *hw);
00187 bool initializeAccel(pr2_hardware_interface::HardwareInterface *hw);
00188 bool initializeFT(pr2_hardware_interface::HardwareInterface *hw);
00189 bool initializeSoftProcessor();
00190
00191 bool unpackPressure(unsigned char* pressure_buf);
00192 bool unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status);
00193 bool unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status);
00194
00195 void diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00196 void diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
00197 void diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status);
00198 void diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
00199
00201 bool has_accel_and_ft_;
00202
00203 bool pressure_checksum_error_;
00204 unsigned pressure_checksum_error_count_;
00205 unsigned pressure_size_;
00206
00207 unsigned accelerometer_samples_;
00208 unsigned accelerometer_missed_samples_;
00209 ros::Time last_publish_time_;
00210 bool first_publish_;
00211
00212 static const unsigned NUM_PRESSURE_REGIONS = 22;
00213 uint32_t last_pressure_time_;
00214 realtime_tools::RealtimePublisher<pr2_msgs::PressureState> *pressure_publisher_;
00215 realtime_tools::RealtimePublisher<pr2_msgs::AccelerometerState> *accel_publisher_;
00216
00217 void convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench);
00218 static const unsigned MAX_FT_SAMPLES = 4;
00219 static const unsigned NUM_FT_CHANNELS = 6;
00220 static const int FT_VHALF_IDEAL = 32768;
00221 static const int FT_VHALF_RANGE = 300;
00222 int ft_overload_limit_;
00223 uint8_t ft_overload_flags_;
00224 bool ft_disconnected_;
00225 bool ft_vhalf_error_;
00226 bool ft_sampling_rate_error_;
00227 uint64_t ft_sample_count_;
00228 uint64_t ft_missed_samples_;
00229 uint64_t diag_last_ft_sample_count_;
00230 pr2_hardware_interface::AnalogIn ft_raw_analog_in_;
00231
00232 pr2_hardware_interface::AnalogIn ft_analog_in_;
00234 pr2_hardware_interface::ForceTorque force_torque_;
00235
00237 realtime_tools::RealtimePublisher<ethercat_hardware::RawFTData> *raw_ft_publisher_;
00238 realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> *ft_publisher_;
00239
00240 FTParamsInternal ft_params_;
00241
00242 bool enable_pressure_sensor_;
00243 bool enable_ft_sensor_;
00244
00250 bool enable_soft_processor_access_;
00251 WGSoftProcessor soft_processor_;
00252 };
00253
00254 #endif