wg06.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_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   //pr2_hardware_interface::AnalogIn ft_analog_in_;      //!< Provides
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 /* ETHERCAT_HARDWARE_WG06_H */


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Jan 2 2014 11:39:31