ethercat_hardware: ethercat_hardware: wg06.h Source 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 <pr2_msgs/PressureState.h>
00041 #include <pr2_msgs/AccelerometerState.h>
00042 #include <ethercat_hardware/RawFTData.h>
00043 #include <geometry_msgs/Wrench.h>
00044 
00045 struct WG06StatusWithAccel
00046 {
00047   uint8_t mode_;
00048   uint8_t digital_out_;
00049   int16_t programmed_pwm_value_;
00050   int16_t programmed_current_;
00051   int16_t measured_current_;
00052   uint32_t timestamp_;
00053   int32_t encoder_count_;
00054   int32_t encoder_index_pos_;
00055   uint16_t num_encoder_errors_;
00056   uint8_t encoder_status_;
00057   uint8_t unused1;
00058   int32_t unused2;
00059   int32_t unused3;
00060   uint16_t board_temperature_;
00061   uint16_t bridge_temperature_;
00062   uint16_t supply_voltage_;
00063   int16_t motor_voltage_;
00064   uint16_t packet_count_;
00065   uint8_t pad_;
00066   uint8_t accel_count_;
00067   uint32_t accel_[4];
00068   uint8_t checksum_;
00069 
00070   static const unsigned SIZE=61;
00071 }__attribute__ ((__packed__));
00072 
00073 
00074 struct FTDataSample
00075 {
00076   int16_t data_[6];
00077   uint16_t vhalf_;
00078   uint8_t sample_count_;
00079   uint8_t timestamp_;
00080   static const unsigned SIZE=16;
00081 }__attribute__ ((__packed__));
00082 
00083 
00084 class FTParamsInternal
00085 {
00086 public:
00087   FTParamsInternal();
00088 
00089   const double &calibration_coeff(unsigned row, unsigned col) const {return calibration_coeff_[row*6 + col];}
00090   double &calibration_coeff(unsigned row, unsigned col) {return calibration_coeff_[row*6 + col];}
00091 
00092   const double &offset(unsigned ch_num) const {return offsets_[ch_num];}
00093   double &offset(unsigned ch_num) {return offsets_[ch_num];}
00094 
00095   const double &gain(unsigned ch_num) const {return gains_[ch_num];}
00096   double &gain(unsigned ch_num) {return gains_[ch_num];}
00097   
00098   void print() const;
00099 
00100   bool getRosParams(ros::NodeHandle nh);
00101   bool getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len);
00102 protected:
00103   double calibration_coeff_[36];
00104   double offsets_[6];
00105   double gains_[6];
00106 };
00107 
00108 
00109 struct WG06StatusWithAccelAndFT
00110 {
00111   uint8_t mode_;
00112   uint8_t digital_out_;
00113   int16_t programmed_pwm_value_;
00114   int16_t programmed_current_;
00115   int16_t measured_current_;
00116   uint32_t timestamp_;
00117   int32_t encoder_count_;
00118   int32_t encoder_index_pos_;
00119   uint16_t num_encoder_errors_;
00120   uint8_t encoder_status_;
00121   uint8_t unused1;
00122   int32_t unused2;
00123   int32_t unused3;
00124   uint16_t board_temperature_;
00125   uint16_t bridge_temperature_;
00126   uint16_t supply_voltage_;
00127   int16_t motor_voltage_;
00128   uint16_t packet_count_;
00129   uint8_t pad_;
00130   uint8_t accel_count_;
00131   uint32_t accel_[4];
00132   uint8_t unused4[3];
00133   uint8_t ft_sample_count_;
00134   FTDataSample ft_samples_[4];
00135   uint8_t checksum_;
00136 
00137   static const unsigned SIZE=129;
00138 }__attribute__ ((__packed__));
00139 
00140 
00141 struct WG06Pressure
00142 {
00143   uint32_t timestamp_;
00144   uint16_t l_finger_tip_[22];
00145   uint16_t r_finger_tip_[22];
00146   uint8_t pad_;
00147   uint8_t checksum_;
00148   static const unsigned SIZE=94;
00149 } __attribute__((__packed__));
00150 
00151 class WG06 : public WG0X
00152 {
00153 public:
00154   WG06();
00155   ~WG06();
00156   int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true);
00157   void construct(EtherCAT_SlaveHandler *sh, int &start_address);
00158   void packCommand(unsigned char *buffer, bool halt, bool reset);
00159   bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
00160 
00161   virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer);
00162   enum
00163   {
00164     PRODUCT_CODE = 6805006
00165   };
00166 private:
00167 
00168   static const unsigned PRESSURE_PHY_ADDR = 0x2200;
00169 
00170   pr2_hardware_interface::PressureSensor pressure_sensors_[2];
00171   pr2_hardware_interface::Accelerometer accelerometer_;
00172 
00173   bool unpackPressure(WG06Pressure *p);
00174   bool unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status);
00175   bool unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status);
00176 
00177   void diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
00178   void diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer);
00179   void diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status);
00180 
00182   bool has_accel_and_ft_;  
00183 
00184   bool pressure_checksum_error_; 
00185 
00186   unsigned accelerometer_samples_; 
00187   unsigned accelerometer_missed_samples_;  
00188   ros::Time last_publish_time_; 
00189   bool first_publish_; 
00190 
00191   uint32_t last_pressure_time_;
00192   realtime_tools::RealtimePublisher<pr2_msgs::PressureState> *pressure_publisher_;
00193   realtime_tools::RealtimePublisher<pr2_msgs::AccelerometerState> *accel_publisher_;
00194 
00195   static const unsigned MAX_FT_SAMPLES = 4;  
00196   static const unsigned NUM_FT_CHANNELS = 6;
00197   uint64_t ft_sample_count_;  
00198   uint64_t ft_missed_samples_;  
00199   uint64_t diag_last_ft_sample_count_; 
00200   pr2_hardware_interface::AnalogIn ft_raw_analog_in_;  
00201   pr2_hardware_interface::AnalogIn ft_analog_in_;  
00202 
00203   realtime_tools::RealtimePublisher<ethercat_hardware::RawFTData> *raw_ft_publisher_;
00204   realtime_tools::RealtimePublisher<geometry_msgs::Wrench> *ft_publisher_;
00205   //pr2_hardware_interface::AnalogIn ft_analog_in_;      //!< Provides
00206   FTParamsInternal ft_params_;
00207 };
00208 
00209 #endif /* ETHERCAT_HARDWARE_WG06_H */