sr06.cpp
Go to the documentation of this file.
00001 
00028 #include <sr_edc_ethercat_drivers/sr06.h>
00029 
00030 #include <dll/ethercat_dll.h>
00031 #include <al/ethercat_AL.h>
00032 #include <dll/ethercat_device_addressed_telegram.h>
00033 #include <dll/ethercat_frame.h>
00034 #include <realtime_tools/realtime_publisher.h>
00035 
00036 #include <math.h>
00037 #include <sstream>
00038 #include <iomanip>
00039 #include <boost/foreach.hpp>
00040 #include <std_msgs/Int16.h>
00041 #include <math.h>
00042 #include <fcntl.h>
00043 #include <stdio.h>
00044 #include <pthread.h>
00045 
00046 #include <sr_utilities/sr_math_utils.hpp>
00047 
00048 using namespace std;
00049 
00050 #include <sr_external_dependencies/types_for_external.h>
00051 
00052 #include <boost/static_assert.hpp>
00053 namespace is_edc_command_32_bits
00054 {
00055   //check is the EDC_COMMAND is 32bits on the computer
00056   //if not, fails
00057   BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
00058 } // namespace is_edc_command_32_bits
00059 
00060 #define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS)
00061 #define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND)
00062 
00063 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
00064 
00065 #define ETHERCAT_COMMAND_DATA_ADDRESS                   PALM_0200_ETHERCAT_COMMAND_DATA_ADDRESS
00066 #define ETHERCAT_STATUS_DATA_ADDRESS                    PALM_0200_ETHERCAT_STATUS_DATA_ADDRESS
00067 #define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS        PALM_0200_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
00068 #define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS         PALM_0200_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
00069 
00070 
00071 
00072 PLUGINLIB_EXPORT_CLASS(SR06, EthercatDevice);
00073 
00074 
00081 SR06::SR06()
00082   : SrEdc(),
00083     zero_buffer_read(0),
00084     cycle_count(0)
00085 {
00086 /*
00087   ROS_INFO("There are %d sensors", nb_sensors_const);
00088   ROS_INFO(     "device_pub_freq_const = %d", device_pub_freq_const      );
00089   ROS_INFO(        "ros_pub_freq_const = %d", ros_pub_freq_const         );
00090   ROS_INFO(            "max_iter_const = %d", max_iter_const             );
00091   ROS_INFO(          "nb_sensors_const = %d", nb_sensors_const           );
00092   ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );
00093 */
00094 }
00095 
00100 SR06::~SR06()
00101 {
00102   delete sh_->get_fmmu_config();
00103   delete sh_->get_pd_config();
00104 }
00105 
00139 void SR06::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00140 {
00141     SrEdc::construct(sh, start_address, ETHERCAT_COMMAND_DATA_SIZE, ETHERCAT_STATUS_DATA_SIZE, ETHERCAT_CAN_BRIDGE_DATA_SIZE,
00142                 ETHERCAT_COMMAND_DATA_ADDRESS, ETHERCAT_STATUS_DATA_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS);
00143 
00144     ROS_INFO("Finished constructing the SR06 driver");
00145 }
00146 
00150 int SR06::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00151 {
00152 
00153   int retval = SrEdc::initialize(hw, allow_unprogrammed);
00154 
00155   if(retval != 0)
00156     return retval;
00157 
00158   sr_hand_lib = boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND> >( new shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>(hw) );
00159 
00160   ROS_INFO("ETHERCAT_STATUS_DATA_SIZE      = %4d bytes", static_cast<int>(ETHERCAT_STATUS_DATA_SIZE) );
00161   ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE     = %4d bytes", static_cast<int>(ETHERCAT_COMMAND_DATA_SIZE) );
00162   ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE  = %4d bytes", static_cast<int>(ETHERCAT_CAN_BRIDGE_DATA_SIZE) );
00163 
00164   //initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
00165   extra_analog_inputs_publisher.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_ , "palm_extras", 10));
00166 
00167 
00168   // Debug real time publisher: publishes the raw ethercat data
00169   debug_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug>(nodehandle_ , "debug_etherCAT_data", 4));
00170   return retval;
00171 }
00172 
00173 
00180 void SR06::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
00181 {
00182   diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
00183 
00184   stringstream name;
00185   d.name = "EtherCAT Dual CAN Palm";
00186   d.summary(d.OK, "OK");
00187   stringstream hwid;
00188   hwid << sh_->get_product_code() << "-" << sh_->get_serial();
00189   d.hardware_id = hwid.str();
00190 
00191   d.clear();
00192   d.addf("Position", "%02d", sh_->get_ring_position());
00193   d.addf("Product Code", "%d", sh_->get_product_code());
00194   d.addf("Serial Number", "%d", sh_->get_serial());
00195   d.addf("Revision", "%d", sh_->get_revision());
00196   d.addf("Counter", "%d", ++counter_);
00197 
00198   d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
00199   d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
00200   //reset the idle time min to a big number, to get a fresh number on next diagnostic
00201   sr_hand_lib->main_pic_idle_time_min = 1000;
00202 
00203   this->ethercatDiagnostics(d,2);
00204   vec.push_back(d);
00205 
00206   //Add the diagnostics from the hand
00207   sr_hand_lib->add_diagnostics(vec, d);
00208 
00209   //Add the diagnostics from the tactiles
00210   if( sr_hand_lib->tactiles != NULL )
00211     sr_hand_lib->tactiles->add_diagnostics(vec, d);
00212 }
00213 
00214 
00215 
00231 void SR06::packCommand(unsigned char *buffer, bool halt, bool reset)
00232 {
00233   SrEdc::packCommand(buffer, halt, reset);
00234 
00235   ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND   *command = (ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND *)(buffer                             );
00236   ETHERCAT_CAN_BRIDGE_DATA                        *message = (ETHERCAT_CAN_BRIDGE_DATA                      *)(buffer + ETHERCAT_COMMAND_DATA_SIZE);
00237 
00238   if ( !flashing )
00239   {
00240     command->EDC_command = EDC_COMMAND_SENSOR_DATA;
00241   }
00242   else
00243   {
00244     command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
00245   }
00246 
00247   //alternate between even and uneven motors
00248   // and ask for the different informations.
00249   sr_hand_lib->build_command(command);
00250 
00251   ROS_DEBUG("Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
00252                 command->to_motor_data_type,
00253                 command->motor_data[0],
00254                 command->motor_data[1],
00255                 command->motor_data[2],
00256                 command->motor_data[3],
00257                 command->motor_data[4],
00258                 command->motor_data[5],
00259                 command->motor_data[6],
00260                 command->motor_data[7],
00261                 command->motor_data[8],
00262                 command->motor_data[9],
00263                 command->motor_data[10],
00264                 command->motor_data[11],
00265                 command->motor_data[12],
00266                 command->motor_data[13],
00267                 command->motor_data[14],
00268                 command->motor_data[15],
00269                 command->motor_data[16],
00270                 command->motor_data[17],
00271                 command->motor_data[18],
00272                 command->motor_data[19]);
00273 
00274   build_CAN_message(message);
00275 }
00276 
00277 
00296 bool SR06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00297 {
00298   ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS  *status_data = (ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS *)(this_buffer + command_size_                             );
00299   ETHERCAT_CAN_BRIDGE_DATA                      *can_data    = (ETHERCAT_CAN_BRIDGE_DATA                     *)(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE );
00300   //  int16u                                        *status_buffer = (int16u*)status_data;
00301   static unsigned int num_rxed_packets = 0;
00302 
00303   ++num_rxed_packets;
00304 
00305 
00306   // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
00307   if(debug_publisher->trylock())
00308   {
00309     debug_publisher->msg_.header.stamp = ros::Time::now();
00310 
00311     debug_publisher->msg_.sensors.clear();
00312     for(unsigned int i=0; i<SENSORS_NUM_0220 + 1; ++i)
00313       debug_publisher->msg_.sensors.push_back( status_data->sensors[i] );
00314 
00315     debug_publisher->msg_.motor_data_type.data = static_cast<int>(status_data->motor_data_type);
00316     debug_publisher->msg_.which_motors = status_data->which_motors;
00317     debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
00318     debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
00319 
00320     debug_publisher->msg_.motor_data_packet_torque.clear();
00321     debug_publisher->msg_.motor_data_packet_misc.clear();
00322     for(unsigned int i=0; i < 10; ++i)
00323     {
00324       debug_publisher->msg_.motor_data_packet_torque.push_back( status_data->motor_data_packet[i].torque );
00325       debug_publisher->msg_.motor_data_packet_misc.push_back( status_data->motor_data_packet[i].misc );
00326     }
00327 
00328     debug_publisher->msg_.tactile_data_type = static_cast<unsigned int>(static_cast<int32u>(status_data->tactile_data_type));
00329     debug_publisher->msg_.tactile_data_valid = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile_data_valid));
00330     debug_publisher->msg_.tactile.clear();
00331     for(unsigned int i=0; i < 5; ++i)
00332       debug_publisher->msg_.tactile.push_back( static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[i].word[0])) );
00333 
00334     debug_publisher->msg_.idle_time_us = status_data->idle_time_us;
00335 
00336     debug_publisher->unlockAndPublish();
00337   }
00338 
00339   if (status_data->EDC_command == EDC_COMMAND_INVALID)
00340   {
00341     //received empty message: the pic is not writing to its mailbox.
00342     ++zero_buffer_read;
00343     float percentage_packet_loss = 100.f * ((float)zero_buffer_read / (float)num_rxed_packets);
00344 
00345     ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus", zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
00346     return true;
00347   }
00348 
00349   //We received a coherent message.
00350   //Update the library (positions, diagnostics values, actuators, etc...)
00351   //with the received information
00352   sr_hand_lib->update(status_data);
00353 
00354   //Now publish the additional data at 100Hz (every 10 cycles)
00355   if( cycle_count >= 9)
00356   {
00357     //publish tactiles if we have them
00358     if( sr_hand_lib->tactiles != NULL )
00359       sr_hand_lib->tactiles->publish();
00360 
00361     //And we also publish the additional data (accelerometer / gyroscope / analog inputs)
00362     std_msgs::Float64MultiArray extra_analog_msg;
00363     extra_analog_msg.layout.dim.resize(3);
00364     extra_analog_msg.data.resize(3+3+4);
00365     std::vector<double> data;
00366 
00367     extra_analog_msg.layout.dim[0].label = "accelerometer";
00368     extra_analog_msg.layout.dim[0].size = 3;
00369     extra_analog_msg.data[0] = status_data->sensors[ACCX];
00370     extra_analog_msg.data[1] = status_data->sensors[ACCY];
00371     extra_analog_msg.data[2] = status_data->sensors[ACCZ];
00372 
00373     extra_analog_msg.layout.dim[1].label = "gyrometer";
00374     extra_analog_msg.layout.dim[1].size = 3;
00375     extra_analog_msg.data[3] = status_data->sensors[GYRX];
00376     extra_analog_msg.data[4] = status_data->sensors[GYRY];
00377     extra_analog_msg.data[5] = status_data->sensors[GYRZ];
00378 
00379     extra_analog_msg.layout.dim[2].label = "analog_inputs";
00380     extra_analog_msg.layout.dim[2].size = 4;
00381     extra_analog_msg.data[6] = status_data->sensors[ANA0];
00382     extra_analog_msg.data[7] = status_data->sensors[ANA1];
00383     extra_analog_msg.data[8] = status_data->sensors[ANA2];
00384     extra_analog_msg.data[9] = status_data->sensors[ANA3];
00385 
00386     if( extra_analog_inputs_publisher->trylock() )
00387     {
00388       extra_analog_inputs_publisher->msg_ = extra_analog_msg;
00389       extra_analog_inputs_publisher->unlockAndPublish();
00390     }
00391 
00392     cycle_count = 0;
00393   }
00394   ++cycle_count;
00395 
00396 
00397   //If we're flashing, check is the packet has been acked
00398   if (flashing & !can_packet_acked)
00399   {
00400     if (can_data_is_ack(can_data))
00401     {
00402       can_packet_acked = true;
00403     }
00404   }
00405 
00406   return true;
00407 }
00408 
00409 void SR06::reinitialize_boards()
00410 {
00411   //Reinitialize motors information
00412   sr_hand_lib->reinitialize_motors();
00413 }
00414 
00415 void SR06::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
00416 {
00417   // We're using 2 can busses,
00418   // if motor id is between 0 and 9, then we're using the can_bus 1
00419   // else, we're using the can bus 2.
00420   int motor_id_tmp = board_id;
00421   if( motor_id_tmp > 9 )
00422   {
00423     motor_id_tmp -= 10;
00424     *can_bus = 2;
00425   }
00426   else
00427   {
00428     *can_bus = 1;
00429   }
00430 
00431   *board_can_id = motor_id_tmp;
00432 }
00433 
00434 /* For the emacs weenies in the crowd.
00435    Local Variables:
00436    c-basic-offset: 2
00437    End:
00438 */


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Thu Aug 27 2015 15:16:54