wg06.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_WG06_H
36 #define ETHERCAT_HARDWARE_WG06_H
37 
38 #include <ethercat_hardware/wg0x.h>
39 
41 
42 #include <pr2_msgs/PressureState.h>
43 #include <pr2_msgs/AccelerometerState.h>
44 #include <ethercat_hardware/RawFTData.h>
45 #include <geometry_msgs/WrenchStamped.h>
46 
48 {
49  uint8_t mode_;
50  uint8_t digital_out_;
54  uint32_t timestamp_;
55  int32_t encoder_count_;
58  uint8_t encoder_status_;
59  uint8_t unused1;
60  int32_t unused2;
61  int32_t unused3;
64  uint16_t supply_voltage_;
65  int16_t motor_voltage_;
66  uint16_t packet_count_;
67  uint8_t pad_;
68  uint8_t accel_count_;
69  uint32_t accel_[4];
70  uint8_t checksum_;
71 
72  static const unsigned SIZE=61;
73 }__attribute__ ((__packed__));
74 
75 
77 {
78  int16_t data_[6];
79  uint16_t vhalf_;
80  uint8_t sample_count_;
81  uint8_t timestamp_;
82  static const unsigned SIZE=16;
83 }__attribute__ ((__packed__));
84 
85 
87 {
88 public:
90 
91  const double &calibration_coeff(unsigned row, unsigned col) const {return calibration_coeff_[row*6 + col];}
92  double &calibration_coeff(unsigned row, unsigned col) {return calibration_coeff_[row*6 + col];}
93 
94  const double &offset(unsigned ch_num) const {return offsets_[ch_num];}
95  double &offset(unsigned ch_num) {return offsets_[ch_num];}
96 
97  const double &gain(unsigned ch_num) const {return gains_[ch_num];}
98  double &gain(unsigned ch_num) {return gains_[ch_num];}
99 
100  void print() const;
101 
102  bool getRosParams(ros::NodeHandle nh);
103  bool getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len);
104  double calibration_coeff_[36];
105  double offsets_[6];
106  double gains_[6];
107 };
108 
109 
111 {
112  uint8_t mode_;
113  uint8_t digital_out_;
117  uint32_t timestamp_;
118  int32_t encoder_count_;
122  uint8_t unused1;
123  int32_t unused2;
124  int32_t unused3;
127  uint16_t supply_voltage_;
128  int16_t motor_voltage_;
129  uint16_t packet_count_;
130  uint8_t pad_;
131  uint8_t accel_count_;
132  uint32_t accel_[4];
133  uint8_t unused4[3];
136  uint8_t checksum_;
137 
138  static const unsigned SIZE=129;
139 }__attribute__ ((__packed__));
140 
141 
143 {
144  uint32_t timestamp_;
145  uint16_t l_finger_tip_[22];
146  uint16_t r_finger_tip_[22];
147  uint8_t pad_;
148  uint8_t checksum_;
149  static const unsigned SIZE=94;
150 } __attribute__((__packed__));
151 
152 
154 {
156  uint8_t pad_[418];
157  uint8_t checksum_;
158  static const unsigned SIZE=513;
159 } __attribute__((__packed__));
160 
161 
162 
163 class WG06 : public WG0X
164 {
165 public:
166  WG06();
167  ~WG06();
168  int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
169  void construct(EtherCAT_SlaveHandler *sh, int &start_address);
170  void packCommand(unsigned char *buffer, bool halt, bool reset);
171  bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
172 
173  virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
174  enum
175  {
176  PRODUCT_CODE = 6805006
177  };
178 private:
179 
180  static const unsigned PRESSURE_PHY_ADDR = 0x2200;
181  static const unsigned BIG_PRESSURE_PHY_ADDR = 0x2600;
182 
185 
186  bool initializePressure(pr2_hardware_interface::HardwareInterface *hw);
187  bool initializeAccel(pr2_hardware_interface::HardwareInterface *hw);
188  bool initializeFT(pr2_hardware_interface::HardwareInterface *hw);
189  bool initializeSoftProcessor();
190 
191  bool unpackPressure(unsigned char* pressure_buf);
192  bool unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status);
193  bool unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status);
194 
195  void diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
196  void diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
198  void diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
199 
202 
205  unsigned pressure_size_;
206 
211 
212  static const unsigned NUM_PRESSURE_REGIONS = 22;
216 
217  void convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench);
218  static const unsigned MAX_FT_SAMPLES = 4;
219  static const unsigned NUM_FT_CHANNELS = 6;
220  static const int FT_VHALF_IDEAL = 32768;
221  static const int FT_VHALF_RANGE = 300;
227  uint64_t ft_sample_count_;
231  pr2_hardware_interface::AnalogIn ft_analog_in_;
235 
239  //pr2_hardware_interface::AnalogIn ft_analog_in_; //!< Provides
241 
244 
252 };
253 
254 #endif /* ETHERCAT_HARDWARE_WG06_H */
bool first_publish_
Definition: wg06.h:210
uint8_t unused1
Definition: wg06.h:59
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > * ft_publisher_
Definition: wg06.h:238
uint8_t accel_count_
Definition: wg06.h:68
uint16_t vhalf_
Definition: wg06.h:79
WGSoftProcessor soft_processor_
Definition: wg06.h:251
realtime_tools::RealtimePublisher< ethercat_hardware::RawFTData > * raw_ft_publisher_
Realtime Publisher of RAW F/T data.
Definition: wg06.h:237
uint16_t packet_count_
Definition: wg06.h:129
bool enable_ft_sensor_
Definition: wg06.h:243
unsigned pressure_size_
Size in bytes of pressure data region.
Definition: wg06.h:205
uint32_t accel_[4]
Definition: wg06.h:69
double & gain(unsigned ch_num)
Definition: wg06.h:98
ROSCONSOLE_DECL void initialize()
int32_t unused3
Definition: wg06.h:61
bool pressure_checksum_error_
Set true where checksum error on pressure data is detected, cleared on reset.
Definition: wg06.h:203
bool ft_vhalf_error_
error with Vhalf reference voltage
Definition: wg06.h:225
ros::Time last_publish_time_
Time diagnostics was last published.
Definition: wg06.h:209
uint16_t supply_voltage_
Definition: wg06.h:64
uint16_t bridge_temperature_
Definition: wg06.h:63
realtime_tools::RealtimePublisher< pr2_msgs::PressureState > * pressure_publisher_
Definition: wg06.h:214
int32_t encoder_index_pos_
Definition: wg06.h:56
int ft_overload_limit_
Limit on raw range of F/T input.
Definition: wg06.h:222
unsigned accelerometer_samples_
Number of accelerometer samples since last publish cycle.
Definition: wg06.h:207
uint8_t digital_out_
Definition: wg06.h:113
int16_t motor_voltage_
Definition: wg06.h:128
uint64_t ft_sample_count_
Counts number of ft sensor samples.
Definition: wg06.h:227
uint32_t timestamp_
Definition: wg06.h:117
int32_t encoder_count_
Definition: wg06.h:118
bool enable_pressure_sensor_
Definition: wg06.h:242
uint16_t bridge_temperature_
Definition: wg06.h:126
uint32_t timestamp_
Definition: wg06.h:54
uint8_t pad_
Definition: wg06.h:67
unsigned accelerometer_missed_samples_
Total of accelerometer samples that were missed.
Definition: wg06.h:208
unsigned pressure_checksum_error_count_
debugging
Definition: wg06.h:204
ROSCONSOLE_DECL void print(FilterBase *filter, void *logger, Level level, const char *file, int line, const char *function, const char *fmt,...) ROSCONSOLE_PRINTF_ATTRIBUTE(7
int16_t motor_voltage_
Definition: wg06.h:65
static const unsigned SIZE
Definition: wg06.h:72
FTParamsInternal ft_params_
Definition: wg06.h:240
pr2_hardware_interface::ForceTorque force_torque_
Provides F/T data to controllers.
Definition: wg06.h:234
uint8_t pad_
Definition: wg06.h:147
class FTParamsInternal __attribute__
int16_t programmed_current_
Definition: wg06.h:52
uint64_t ft_missed_samples_
Counts number of ft sensor samples that were missed.
Definition: wg06.h:228
pr2_hardware_interface::AnalogIn ft_raw_analog_in_
Definition: wg06.h:230
bool ft_disconnected_
f/t sensor may be disconnected
Definition: wg06.h:224
uint64_t diag_last_ft_sample_count_
F/T Sample count last time diagnostics was published.
Definition: wg06.h:229
int32_t encoder_index_pos_
Definition: wg06.h:119
uint16_t num_encoder_errors_
Definition: wg06.h:57
int16_t measured_current_
Definition: wg06.h:116
uint16_t status
const double & offset(unsigned ch_num) const
Definition: wg06.h:94
pr2_hardware_interface::Accelerometer accelerometer_
Definition: wg06.h:184
uint8_t ft_sample_count_
Definition: wg06.h:134
uint32_t timestamp_
Definition: wg06.h:144
int16_t programmed_pwm_value_
Definition: wg06.h:114
WG06Pressure pressure_
Definition: wg06.h:155
uint16_t supply_voltage_
Definition: wg06.h:127
uint8_t encoder_status_
Definition: wg06.h:121
uint16_t board_temperature_
Definition: wg06.h:62
uint8_t encoder_status_
Definition: wg06.h:58
bool has_accel_and_ft_
True if device has accelerometer and force/torque sensor.
Definition: wg06.h:201
int16_t programmed_pwm_value_
Definition: wg06.h:51
int16_t data_[6]
Definition: wg06.h:50
uint8_t digital_out_
Definition: wg06.h:50
uint8_t checksum_
Definition: wg06.h:148
realtime_tools::RealtimePublisher< pr2_msgs::AccelerometerState > * accel_publisher_
Definition: wg06.h:215
const double & gain(unsigned ch_num) const
Definition: wg06.h:97
uint16_t r_finger_tip_[22]
Definition: wg06.h:52
uint8_t mode_
Definition: wg06.h:49
uint8_t ft_overload_flags_
Bits 0-5 set to true if raw FT input goes beyond limit.
Definition: wg06.h:223
uint16_t num_encoder_errors_
Definition: wg06.h:120
uint16_t l_finger_tip_[22]
Definition: wg06.h:51
uint8_t unused4[3]
Definition: wg06.h:71
int16_t programmed_current_
Definition: wg06.h:115
double & calibration_coeff(unsigned row, unsigned col)
Definition: wg06.h:92
uint8_t sample_count_
Definition: wg06.h:80
uint8_t accel_count_
Definition: wg06.h:131
Definition: wg0x.h:234
int16_t measured_current_
Definition: wg06.h:53
const double & calibration_coeff(unsigned row, unsigned col) const
Definition: wg06.h:91
FTDataSample ft_samples_[4]
Definition: wg06.h:73
uint16_t board_temperature_
Definition: wg06.h:125
double & offset(unsigned ch_num)
Definition: wg06.h:95
bool enable_soft_processor_access_
Definition: wg06.h:250
uint8_t timestamp_
Definition: wg06.h:81
uint8_t checksum_
Definition: wg06.h:70
int32_t encoder_count_
Definition: wg06.h:55
uint16_t packet_count_
Definition: wg06.h:66
int32_t unused2
Definition: wg06.h:60
bool ft_sampling_rate_error_
True if FT sampling rate was incorrect.
Definition: wg06.h:226
uint8_t checksum_
Definition: wg06.h:157
uint32_t last_pressure_time_
Definition: wg06.h:213
Definition: wg06.h:163


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