00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00024 #include "../include/sr_ronex_drivers/sr_board_dc_motor_small.hpp"
00025
00026 #include <ros_ethercat_model/robot_state.hpp>
00027 #include <ros_ethercat_hardware/ethercat_hardware.h>
00028
00029 #include <sstream>
00030 #include <iomanip>
00031 #include <boost/lexical_cast.hpp>
00032 #include <math.h>
00033 #include <string>
00034
00035 #include "sr_ronex_drivers/ronex_utils.hpp"
00036
00037 PLUGINLIB_EXPORT_CLASS(SrBoardDCMOTORSMALL, EthercatDevice);
00038
00039 const std::string SrBoardDCMOTORSMALL::product_alias_ = "DC_MOTOR_SMALL";
00040 using boost::lexical_cast;
00041
00042
00043 SrBoardDCMOTORSMALL::SrBoardDCMOTORSMALL() :
00044 node_("~"), cycle_count_(0), has_stacker_(false)
00045 {}
00046
00047 SrBoardDCMOTORSMALL::~SrBoardDCMOTORSMALL()
00048 {
00049
00050 string device_id = "/ronex/devices/" + lexical_cast<string>(parameter_id_);
00051 ros::param::del(device_id);
00052
00053 string DC_MOTOR_SMALL_device_name = "/ronex/DC_MOTOR_SMALL/" + serial_number_;
00054 ros::param::del(DC_MOTOR_SMALL_device_name);
00055
00056 string controller_name = "/ronex_" + serial_number_ + "_passthrough";
00057 }
00058
00059 void SrBoardDCMOTORSMALL::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00060 {
00061 sh_ = sh;
00062 serial_number_ = ronex::get_serial_number(sh);
00063
00064
00065 std::string path_to_alias, alias;
00066 path_to_alias = "/ronex/mapping/" + serial_number_;
00067 if ( ros::param::get(path_to_alias, alias))
00068 {
00069 ronex_id_ = alias;
00070 }
00071 else
00072 {
00073
00074 ronex_id_ = serial_number_;
00075 }
00076
00077 device_name_ = ronex::build_name(product_alias_, ronex_id_);
00078
00079 command_base_ = start_address;
00080 command_size_ = COMMAND_ARRAY_SIZE_BYTES_02000009;
00081
00082 start_address += command_size_;
00083 status_base_ = start_address;
00084 status_size_ = STATUS_ARRAY_SIZE_BYTES_02000009;
00085
00086 start_address += status_size_;
00087
00088
00089
00090
00091
00092
00093 if ( (PROTOCOL_TYPE) == (EC_BUFFERED) )
00094 {
00095 ROS_INFO("Using EC_BUFFERED");
00096 }
00097 else if ( (PROTOCOL_TYPE) == (EC_QUEUED) )
00098 {
00099 ROS_INFO("Using EC_QUEUED");
00100 }
00101
00102 ROS_INFO("First FMMU (command) : Logical address: 0x%08X ; size: %3d bytes ; ET1200 address: 0x%08X", command_base_,
00103 command_size_, static_cast<int>(COMMAND_ADDRESS_02000009) );
00104 EC_FMMU *commandFMMU = new EC_FMMU( command_base_,
00105 command_size_,
00106 0x00,
00107 0x07,
00108 COMMAND_ADDRESS_02000009,
00109 0x00,
00110 false,
00111 true,
00112 true);
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126 ROS_INFO("Second FMMU (status) : Logical address: 0x%08X ; size: %3d bytes ; ET1200 address: 0x%08X", status_base_,
00127 status_size_, static_cast<int>(STATUS_ADDRESS_02000009) );
00128 EC_FMMU *statusFMMU = new EC_FMMU( status_base_,
00129 status_size_,
00130 0x00,
00131 0x07,
00132 STATUS_ADDRESS_02000009,
00133 0x00,
00134 true,
00135 false,
00136 true);
00137
00138
00139 EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00140
00141 (*fmmu)[0] = *commandFMMU;
00142 (*fmmu)[1] = *statusFMMU;
00143
00144 sh->set_fmmu_config(fmmu);
00145
00146 EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(2);
00147
00148
00149 (*pd)[0] = EC_SyncMan(COMMAND_ADDRESS_02000009, command_size_, PROTOCOL_TYPE, EC_WRITTEN_FROM_MASTER);
00150 (*pd)[1] = EC_SyncMan(STATUS_ADDRESS_02000009, status_size_, PROTOCOL_TYPE);
00151
00152
00153 (*pd)[0].ChannelEnable = true;
00154 (*pd)[0].ALEventEnable = true;
00155 (*pd)[0].WriteEvent = true;
00156
00157 (*pd)[1].ChannelEnable = true;
00158
00159 sh->set_pd_config(pd);
00160
00161 ROS_INFO("Finished constructing the SrBoardDC_MOTOR_SMALL driver");
00162 }
00163
00164 int SrBoardDCMOTORSMALL::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00165 {
00166 digital_commands_ = 0;
00167 ROS_INFO("Device #%02d: Product code: %u (%#010X) , Serial #: %u (%#010X)",
00168 sh_->get_ring_position(),
00169 sh_->get_product_code(),
00170 sh_->get_product_code(),
00171 sh_->get_serial(),
00172 sh_->get_serial());
00173
00174 device_offset_ = sh_->get_ring_position();
00175
00176
00177 ros_ethercat_model::RobotState *robot_state = static_cast<ros_ethercat_model::RobotState*>(hw);
00178 robot_state->custom_hws_.insert(device_name_, new ronex::DCMotor());
00179 dc_motor_small_ = static_cast<ronex::DCMotor*>(robot_state->getCustomHW(device_name_));
00180
00181 build_topics_();
00182
00183 ROS_INFO_STREAM("Adding an DC_MOTOR_SMALL RoNeX module to the hardware interface: " << device_name_);
00184
00185
00186 return 0;
00187 }
00188
00189 void SrBoardDCMOTORSMALL::packCommand(unsigned char *buffer, bool halt, bool reset)
00190 {
00191 RONEX_COMMAND_02000009* command = reinterpret_cast<RONEX_COMMAND_02000009*>(buffer);
00192
00193 command->command_type = RONEX_COMMAND_02000009_COMMAND_TYPE_NORMAL;
00194
00195 for (size_t i = 0; i < dc_motor_small_->command_.digital_.size(); ++i)
00196 {
00197 if (input_mode_[i])
00198 {
00199
00200 ronex::set_bit(digital_commands_, i*2, 1);
00201 }
00202 else
00203 {
00204 ronex::set_bit(digital_commands_, i*2, 0);
00205 ronex::set_bit(digital_commands_, i*2+1, dc_motor_small_->command_.digital_[i]);
00206 }
00207 }
00208 command->pin_output_states_DIO = static_cast<int16u>(digital_commands_);
00209
00210 for (size_t i = 0; i < dc_motor_small_->command_.motor_packet_command_.size(); ++i)
00211 {
00212 command->motor_packet_command[i].flags = dc_motor_small_->command_.motor_packet_command_[i].flags;
00213 command->motor_packet_command[i].onTime = dc_motor_small_->command_.motor_packet_command_[i].on_time;
00214 command->motor_packet_command[i].period = dc_motor_small_->command_.motor_packet_command_[i].period;
00215 }
00216 }
00217
00218 bool SrBoardDCMOTORSMALL::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00219 {
00220 RONEX_STATUS_02000009* status_data = reinterpret_cast<RONEX_STATUS_02000009 *>(this_buffer+ command_size_);
00221 if (status_data->command_type == RONEX_COMMAND_02000009_COMMAND_TYPE_NORMAL)
00222 {
00223 if (dc_motor_small_->state_.analogue_.empty())
00224 {
00225 size_t nb_analogue_pub, nb_digital_io, nb_motor_packet;
00226
00227
00228
00229 nb_analogue_pub = NUM_ANALOGUE_INPUTS_02000009;
00230 nb_digital_io = NUM_DIGITAL_IO_02000009;
00231 if (status_data->motor_packet_status[0].flags & RONEX_02000009_FLAGS_STACKER_1_PRESENT)
00232 {
00233 has_stacker_ = true;
00234 nb_motor_packet = 2;
00235 }
00236 else
00237 {
00238 has_stacker_ = false;
00239 nb_motor_packet = 1;
00240 }
00241
00242 dc_motor_small_->state_.analogue_.resize(nb_analogue_pub);
00243 dc_motor_small_->state_.digital_.resize(nb_digital_io);
00244 dc_motor_small_->state_.motor_packet_status_.resize(nb_motor_packet);
00245 dc_motor_small_->command_.digital_.resize(nb_digital_io);
00246 dc_motor_small_->command_.motor_packet_command_.resize(nb_motor_packet);
00247
00248 input_mode_.assign(nb_digital_io, true);
00249
00250
00251 state_msg_.analogue.resize(nb_analogue_pub);
00252 state_msg_.digital.resize(nb_digital_io);
00253 state_msg_.input_mode.resize(nb_digital_io);
00254 state_msg_.motor_status.resize(nb_motor_packet);
00255
00256
00257
00258
00259 dynamic_reconfigure_server_.reset(
00260 new dynamic_reconfigure::Server<sr_ronex_drivers::DCMotorConfig>(ros::NodeHandle(device_name_)));
00261 function_cb_ = boost::bind(&SrBoardDCMOTORSMALL::dynamic_reconfigure_cb, this, _1, _2);
00262 dynamic_reconfigure_server_->setCallback(function_cb_);
00263 }
00264
00265 for (size_t i = 0; i < dc_motor_small_->state_.analogue_.size(); ++i )
00266 {
00267 dc_motor_small_->state_.analogue_[i] = status_data->analogue_in[i];
00268 }
00269
00270 for (size_t i = 0; i < dc_motor_small_->state_.digital_.size(); ++i )
00271 {
00272 dc_motor_small_->state_.digital_[i] = ronex::check_bit(status_data->pin_input_states_DIO, i);
00273 }
00274 for (size_t i = 0; i < dc_motor_small_->state_.motor_packet_status_.size(); ++i)
00275 {
00276 dc_motor_small_->state_.motor_packet_status_[i].flags = status_data->motor_packet_status[i].flags;
00277 dc_motor_small_->state_.motor_packet_status_[i].quadrature = status_data->motor_packet_status[i].quadrature;
00278 }
00279 }
00280
00281 if (cycle_count_ > 9)
00282 {
00283 state_msg_.header.stamp = ros::Time::now();
00284
00285
00286 for (size_t i = 0; i < dc_motor_small_->state_.analogue_.size(); ++i)
00287 {
00288 state_msg_.analogue[i] = dc_motor_small_->state_.analogue_[i];
00289 }
00290
00291 for (size_t i = 0; i < dc_motor_small_->state_.digital_.size(); ++i)
00292 {
00293 state_msg_.digital[i] = dc_motor_small_->state_.digital_[i];
00294 state_msg_.input_mode[i] = input_mode_[i];
00295 }
00296
00297 for (size_t i = 0; i < dc_motor_small_->state_.motor_packet_status_.size(); ++i)
00298 {
00299 state_msg_.motor_status[i].flags = dc_motor_small_->state_.motor_packet_status_[i].flags;
00300 state_msg_.motor_status[i].quadrature = dc_motor_small_->state_.motor_packet_status_[i].quadrature;
00301 }
00302
00303 if ( state_publisher_->trylock() )
00304 {
00305 state_publisher_->msg_ = state_msg_;
00306 state_publisher_->unlockAndPublish();
00307 }
00308
00309 cycle_count_ = 0;
00310 }
00311
00312 cycle_count_++;
00313 return true;
00314 }
00315
00316 void SrBoardDCMOTORSMALL::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00317 {
00318 d.name = device_name_;
00319 d.summary(d.OK, "OK");
00320 d.hardware_id = serial_number_;
00321
00322 d.clear();
00323 if (has_stacker_)
00324 d.addf("Stacker Board", "True");
00325 else
00326 d.addf("Stacker Board", "False");
00327 }
00328
00329 void SrBoardDCMOTORSMALL::dynamic_reconfigure_cb(sr_ronex_drivers::DCMotorConfig &config, uint32_t level)
00330 {
00331 if (dc_motor_small_->command_.digital_.size() > 0)
00332 input_mode_[0] = config.input_mode_0;
00333 if (dc_motor_small_->command_.digital_.size() > 1)
00334 input_mode_[1] = config.input_mode_1;
00335 if (dc_motor_small_->command_.digital_.size() > 2)
00336 input_mode_[2] = config.input_mode_2;
00337 if (dc_motor_small_->command_.digital_.size() > 3)
00338 input_mode_[3] = config.input_mode_3;
00339 if (dc_motor_small_->command_.digital_.size() > 4)
00340 input_mode_[4] = config.input_mode_4;
00341 if (dc_motor_small_->command_.digital_.size() > 5)
00342 input_mode_[5] = config.input_mode_5;
00343 }
00344
00345 void SrBoardDCMOTORSMALL::build_topics_()
00346 {
00347
00348 parameter_id_ = ronex::get_ronex_param_id("");
00349 std::stringstream param_path, tmp_param;
00350 param_path << "/ronex/devices/" << parameter_id_ << "/";
00351 tmp_param << ronex::get_product_code(sh_);
00352 ros::param::set(param_path.str() + "product_id", tmp_param.str());
00353 ros::param::set(param_path.str() + "product_name", product_alias_);
00354 ros::param::set(param_path.str() + "ronex_id", ronex_id_);
00355
00356
00357 ros::param::set(param_path.str() + "path", device_name_);
00358 ros::param::set(param_path.str() + "serial", serial_number_);
00359
00360
00361 state_publisher_.reset(new realtime_tools::RealtimePublisher<sr_ronex_msgs::DCMotorState>(node_, device_name_ +
00362 "/state", 1));
00363 }
00364
00365
00366
00367
00368
00369