Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00024 #include "sr_ronex_transmissions/mapping/general_io/command_to_pwm.hpp"
00025 #include <ros_ethercat_model/robot_state.hpp>
00026 #include <boost/lexical_cast.hpp>
00027 #include <math.h>
00028
00029 namespace ronex
00030 {
00031 namespace mapping
00032 {
00033 namespace general_io
00034 {
00035 CommandToPWM::CommandToPWM(TiXmlElement* mapping_el, ros_ethercat_model::RobotState* robot)
00036 : RonexMapping(), pin_out_of_bound_(true)
00037 {
00038 const char *ronex_name = mapping_el ? mapping_el->Attribute("ronex") : NULL;
00039 if (!ronex_name)
00040 {
00041 ROS_ERROR("RonexTransmission transmission did not specify the ronex name");
00042 return;
00043 }
00044
00045 init_timer_ = nh_.createTimer(ros::Duration(0.01),
00046 boost::bind(&CommandToPWM::try_init_cb_, this, _1, mapping_el, robot, ronex_name));
00047 }
00048
00049 bool CommandToPWM::try_init_cb_(const ros::TimerEvent&, TiXmlElement* mapping_el, ros_ethercat_model::RobotState* robot,
00050 const char* ronex_name)
00051 {
00052 if ( !init_(mapping_el, robot, ronex_name) )
00053 {
00054 return false;
00055 }
00056
00057 ROS_DEBUG_STREAM("RoNeX" << ronex_name << " is initialised now.");
00058
00059 init_timer_.stop();
00060
00061 is_initialized_ = true;
00062 return true;
00063 }
00064
00065 bool CommandToPWM::init_(TiXmlElement* mapping_el, ros_ethercat_model::RobotState* robot, const char* ronex_name)
00066 {
00067
00068 if ( robot->getCustomHW(ronex_name) == NULL )
00069 return false;
00070
00071 general_io_ = static_cast<ronex::GeneralIO*>(robot->getCustomHW(ronex_name));
00072 if (!general_io_)
00073 {
00074 ROS_ERROR_STREAM("The RoNeX: " << ronex_name << " was not found on the system.");
00075 return false;
00076 }
00077
00078
00079 const char *pwm_module = mapping_el ? mapping_el->Attribute("pwm_module") : NULL;
00080 if (!pwm_module)
00081 {
00082 ROS_ERROR("RonexTransmission transmission did not specify the pwm module.");
00083 return false;
00084 }
00085
00086 try
00087 {
00088 pwm_module_ = boost::lexical_cast<size_t>(pwm_module);
00089 }
00090 catch( boost::bad_lexical_cast const& )
00091 {
00092 ROS_ERROR("RonexTransmission: Couldn't parse pwm_module to a size_t.");
00093 return false;
00094 }
00095
00096
00097 const char *pin = mapping_el ? mapping_el->Attribute("pwm_pin") : NULL;
00098 if (!pin)
00099 {
00100 ROS_ERROR("RonexTransmission transmission did not specify the pwm pin.");
00101 return false;
00102 }
00103
00104 try
00105 {
00106 pwm_pin_index_ = boost::lexical_cast<size_t>(pin);
00107 }
00108 catch( boost::bad_lexical_cast const& )
00109 {
00110 ROS_ERROR("RonexTransmission: Couldn't parse pwm_pin to a size_t.");
00111 return false;
00112 }
00113
00114
00115 const char *d_pin = mapping_el ? mapping_el->Attribute("direction_pin") : NULL;
00116 if (!d_pin)
00117 {
00118 ROS_ERROR("RonexTransmission transmission did not specify the direction pin.");
00119 return false;
00120 }
00121
00122 try
00123 {
00124 digital_pin_index_ = boost::lexical_cast<size_t>(d_pin);
00125 }
00126 catch( boost::bad_lexical_cast const& )
00127 {
00128 ROS_ERROR("RonexTransmission: Couldn't parse direction_pin to a size_t.");
00129 return false;
00130 }
00131
00132 return true;
00133 }
00134
00135 bool CommandToPWM::check_pins_in_bound_()
00136 {
00137
00138 if ( first_iteration_ )
00139 {
00140 pin_out_of_bound_ = true;
00141 first_iteration_ = false;
00142 return false;
00143 }
00144
00145
00146 if ( pin_out_of_bound_ )
00147 {
00148 if ( pwm_module_ >= general_io_->command_.pwm_.size() )
00149 {
00150
00151 ROS_ERROR_STREAM("Specified PWM module index is out of bound: " << pwm_module_ << " / max = " <<
00152 general_io_->command_.pwm_.size() << ", not propagating the command to the RoNeX.");
00153 pin_out_of_bound_ = true;
00154 return false;
00155 }
00156 if ( pwm_pin_index_ > 1 )
00157 {
00158
00159 ROS_ERROR_STREAM("Specified PWM pin is out of bound: " << pwm_pin_index_ <<
00160 " / max = 1, not propagating the command to the RoNeX.");
00161 pin_out_of_bound_ = true;
00162 return false;
00163 }
00164 if ( digital_pin_index_ > general_io_->command_.digital_.size() )
00165 {
00166
00167 ROS_ERROR_STREAM("Specified direction pin is out of bound: " << digital_pin_index_ << " / max = " <<
00168 general_io_->command_.digital_.size() << " , not propagating the command to the RoNeX.");
00169 pin_out_of_bound_ = true;
00170 return false;
00171 }
00172 }
00173
00174 pin_out_of_bound_ = false;
00175 return true;
00176 }
00177
00178 void CommandToPWM::propagateToRonex(ros_ethercat_model::JointState *js)
00179 {
00180 if ( !is_initialized_ )
00181 return;
00182
00183 if ( check_pins_in_bound_() )
00184 {
00185 if ( pwm_pin_index_ == 0 )
00186 general_io_->command_.pwm_[pwm_module_].on_time_0 =
00187 static_cast<uint16_t>((static_cast<double>(general_io_->command_.pwm_[pwm_module_].period) *
00188 abs(js->commanded_effort_) ) / 100);
00189 else
00190 general_io_->command_.pwm_[pwm_module_].on_time_1 =
00191 static_cast<uint16_t>((static_cast<double>(general_io_->command_.pwm_[pwm_module_].period) *
00192 abs(js->commanded_effort_) ) / 100);
00193
00194
00195 general_io_->command_.digital_[digital_pin_index_] = (js->commanded_effort_ < 0.0);
00196 }
00197 }
00198 }
00199 }
00200 }
00201
00202
00203
00204
00205