$search
00001 00027 #ifndef _SR_ACTUATOR_HPP_ 00028 #define _SR_ACTUATOR_HPP_ 00029 00030 #include "sr_hardware_interface/tactile_sensors.hpp" 00031 #include <pr2_hardware_interface/hardware_interface.h> 00032 00033 namespace sr_actuator 00034 { 00035 class SrActuatorState : public pr2_hardware_interface::ActuatorState 00036 { 00037 public: 00038 SrActuatorState() : 00039 pr2_hardware_interface::ActuatorState(), 00040 strain_gauge_left_(0), 00041 strain_gauge_right_(0), 00042 temperature_(0), 00043 position_unfiltered_(0.0), 00044 force_unfiltered_(0.0), 00045 pwm_(0), 00046 can_msgs_received_(0), 00047 can_msgs_transmitted_(0), 00048 pic_firmware_svn_revision_(0), 00049 server_firmware_svn_revision_(0), 00050 firmware_modified_(0), 00051 serial_number_low_set(false), 00052 serial_number_high_set(false), 00053 serial_number_low(0), 00054 serial_number_high(0), 00055 serial_number(0), 00056 motor_gear_ratio(0), 00057 assembly_data_year(0), 00058 assembly_data_month(0), 00059 assembly_data_day(0), 00060 tests_(0), 00061 can_error_counters(0), 00062 force_control_f_(0), 00063 force_control_p_(0), 00064 force_control_i_(0), 00065 force_control_d_(0), 00066 force_control_imax_(0), 00067 force_control_deadband_(0), 00068 force_control_sign_(0), 00069 force_control_frequency_(0) 00070 {} 00071 00072 signed short strain_gauge_left_; 00073 signed short strain_gauge_right_; 00074 00075 std::vector<int> raw_sensor_values_; 00076 std::vector<double> calibrated_sensor_values_; 00077 00085 std::vector<std::pair<std::string, bool> > flags_; 00086 00087 double temperature_; 00088 00089 double position_unfiltered_; 00090 double force_unfiltered_; 00091 00092 int pwm_; 00093 00094 uint64_t can_msgs_received_; 00095 uint64_t can_msgs_transmitted_; 00096 00097 unsigned int pic_firmware_svn_revision_; 00098 unsigned int server_firmware_svn_revision_; 00099 bool firmware_modified_; 00100 00101 //Serial number is composed of a low and 00102 // a high byte. 00103 void set_serial_number_low(unsigned int serial) 00104 { 00105 serial_number_low = serial; 00106 00107 serial_number_low_set = true; 00108 if( serial_number_high_set ) //we received both bytes 00109 compute_serial(); 00110 }; 00111 void set_serial_number_high(unsigned int serial) 00112 { 00113 serial_number_high = serial; 00114 00115 serial_number_high_set = true; 00116 if( serial_number_low_set ) //we received both bytes 00117 compute_serial(); 00118 }; 00119 void compute_serial() 00120 { 00121 serial_number = serial_number_low + (serial_number_high * 65536); 00122 }; 00123 00124 bool serial_number_low_set; 00125 bool serial_number_high_set; 00126 unsigned int serial_number_low; 00127 unsigned int serial_number_high; 00128 unsigned int serial_number; 00129 00130 unsigned int motor_gear_ratio; 00131 unsigned int assembly_data_year; 00132 unsigned int assembly_data_month; 00133 unsigned int assembly_data_day; 00134 00135 int tests_; 00136 unsigned int can_error_counters; 00137 00138 int force_control_f_; 00139 int force_control_p_; 00140 int force_control_i_; 00141 int force_control_d_; 00142 int force_control_imax_; 00143 int force_control_deadband_; 00144 int force_control_sign_; 00145 int force_control_frequency_; 00146 00147 int force_control_pterm; 00148 int force_control_iterm; 00149 int force_control_dterm; 00150 00151 std::vector<tactiles::AllTactileData>* tactiles_; 00152 }; //end class SrActuatorState 00153 00154 00155 00156 class SrActuator : public pr2_hardware_interface::Actuator 00157 { 00158 public: 00159 SrActuator() 00160 : pr2_hardware_interface::Actuator() 00161 {}; 00162 00163 SrActuator(std::string name) 00164 : pr2_hardware_interface::Actuator(name) 00165 {}; 00166 00167 SrActuatorState state_; 00168 }; //end class SrActuator 00169 } 00170 00171 /* For the emacs weenies in the crowd. 00172 Local Variables: 00173 c-basic-offset: 2 00174 End: 00175 */ 00176 00177 #endif 00178