motor_updater.cpp
Go to the documentation of this file.
00001 
00029 #include "sr_robot_lib/motor_updater.hpp"
00030 #include <boost/foreach.hpp>
00031 #include <iostream>
00032 
00033 namespace generic_updater
00034 {
00035   template <class CommandType>
00036   MotorUpdater<CommandType>::MotorUpdater(std::vector<UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00037     : GenericUpdater<CommandType>(update_configs_vector, update_state), even_motors(1)
00038   {
00039   }
00040 
00041   template <class CommandType>
00042   MotorUpdater<CommandType>::~MotorUpdater()
00043   {
00044   }
00045 
00046   template <class CommandType>
00047   operation_mode::device_update_state::DeviceUpdateState MotorUpdater<CommandType>::build_init_command(CommandType* command)
00048   {
00049     if(!this->mutex->try_lock())
00050       return this->update_state;
00051 
00052     if (this->update_state == operation_mode::device_update_state::INITIALIZATION)
00053     {
00055       // First we ask for the next data we want to receive
00056       if(even_motors)
00057         even_motors = 0;
00058       else
00059       {
00060         even_motors = 1;
00061         this->which_data_to_request ++;
00062 
00063         if( this->which_data_to_request >= this->initialization_configs_vector.size() )
00064           this->which_data_to_request = 0;
00065       }
00066 
00067       command->which_motors = even_motors;
00068 
00069       //initialization data
00070       command->from_motor_data_type = static_cast<FROM_MOTOR_DATA_TYPE>(this->initialization_configs_vector[this->which_data_to_request].what_to_update);
00071       ROS_DEBUG_STREAM("Updating initialization data type: "<<command->from_motor_data_type << " | ["<<this->which_data_to_request<<"/"<<this->initialization_configs_vector.size()<<"] ");
00072     }
00073     else
00074     {
00075       //For the last message sent when a change of update_state happens (after that we use build_command instead of build_init_command)
00076       //we use the first important message and ask it to the even motors (0)
00077       //This is to avoid sending a random command
00078       command->which_motors = 0;
00079       command->from_motor_data_type = static_cast<FROM_MOTOR_DATA_TYPE>(this->important_update_configs_vector[0].what_to_update);
00080       ROS_DEBUG_STREAM("Updating important data type: "<<command->from_motor_data_type << " | ["<<this->which_data_to_request<<"/"<<this->important_update_configs_vector.size()<<"] ");
00081     }
00082 
00083     this->mutex->unlock();
00084 
00085     return this->update_state;
00086   }
00087 
00088   template <class CommandType>
00089   operation_mode::device_update_state::DeviceUpdateState MotorUpdater<CommandType>::build_command(CommandType* command)
00090   {
00091     if(!this->mutex->try_lock())
00092       return this->update_state;
00093 
00095     // First we ask for the next data we want to receive
00096     if(even_motors)
00097       even_motors = 0;
00098     else
00099     {
00100       even_motors = 1;
00101       this->which_data_to_request ++;
00102 
00103       if( this->which_data_to_request >= this->important_update_configs_vector.size() )
00104         this->which_data_to_request = 0;
00105     }
00106 
00107     command->which_motors = even_motors;
00108 
00109     if(!this->unimportant_data_queue.empty())
00110     {
00111       //an unimportant data is available
00112       command->from_motor_data_type = static_cast<FROM_MOTOR_DATA_TYPE>(this->unimportant_data_queue.front());
00113       this->unimportant_data_queue.pop();
00114 
00115       ROS_DEBUG_STREAM("Updating unimportant data type: "<<command->from_motor_data_type << " | queue size: "<<this->unimportant_data_queue.size());
00116     }
00117     else
00118     {
00119       //important data to update as often as possible
00120       command->from_motor_data_type = static_cast<FROM_MOTOR_DATA_TYPE>(this->important_update_configs_vector[this->which_data_to_request].what_to_update);
00121       ROS_DEBUG_STREAM("Updating important data type: "<<command->from_motor_data_type << " | ["<<this->which_data_to_request<<"/"<<this->important_update_configs_vector.size()<<"] ");
00122     }
00123 
00124     this->mutex->unlock();
00125 
00126     return this->update_state;
00127   }
00128 
00129   //Only to ensure that the template class is compiled for the types we are interested in
00130   template class MotorUpdater<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00131   template class MotorUpdater<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00132 }
00133 
00134 
00135 
00136 /* For the emacs weenies in the crowd.
00137    Local Variables:
00138    c-basic-offset: 2
00139    End:
00140 */


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50