motioncontroller.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    MotionController.cpp
00004   * @author  RBRO/PJ-IU
00005   * @version V1.0.0
00006   * @date    day-month-year
00007   * @brief   This file contains the class definition for the motion controller
00008   *          functionality.
00009   ******************************************************************************
00010  */
00011 
00012 #include <MotionController/motioncontroller.hpp>
00013 
00014 
00015 /** \brief  CMotionController Class constructor
00016  *
00017  *  Constructor method
00018  *
00019  *  @param f_period_sec          period for controller execution in seconds
00020  *  @param f_serialPort          reference to serial communication object
00021  *  @param f_car                 reference to MOVE object
00022  *  @param f_safetyStop          reference to spline safery stop object
00023  *  @param f_control             reference to controller object
00024  */
00025 CMotionController::CMotionController(
00026         float f_period_sec,
00027         Serial& f_serialPort,
00028         Move& f_car,
00029         CSafetyStopFunction* f_safetyStop,
00030         controllers::CControllerSiso* f_control) 
00031     : m_serialPort(f_serialPort)
00032     , m_car(f_car)
00033     , m_speed()
00034     , m_angle()
00035     , m_period_sec(f_period_sec)
00036     , m_isSplineActivated(false)
00037     , m_ispidActivated(false)
00038     , m_motionPlanner()
00039     , m_hbTimeOut()
00040     , m_control(f_control)
00041     , m_safetyStop(f_safetyStop)
00042     , m_timer(mbed::callback(CMotionController::staticCallbackRun,this))
00043 {
00044 }
00045 
00046 
00047 /** \brief  CMotionController Class constructor
00048  *
00049  *  Constructor method
00050  *
00051  *  @param f_period_sec          period for controller execution in seconds
00052  *  @param f_serialPort          reference to serial communication object
00053  *  @param f_car                 reference to MOVE object
00054  *  @param f_control             reference to controller object
00055  */
00056 CMotionController::CMotionController(
00057         float f_period_sec,
00058         Serial& f_serialPort,
00059         Move& f_car,
00060         controllers::CControllerSiso*           f_control) 
00061     : m_serialPort(f_serialPort)
00062     , m_car(f_car)
00063     , m_speed()
00064     , m_angle()
00065     , m_period_sec(f_period_sec)
00066     , m_isSplineActivated(false)
00067     , m_ispidActivated(false)
00068     , m_motionPlanner()
00069     , m_hbTimeOut()
00070     , m_control(f_control)
00071     , m_safetyStop(NULL)
00072     , m_timer(mbed::callback(CMotionController::staticCallbackRun,this))
00073 {
00074 }
00075 
00076 /** \brief  Serial callback method
00077   *
00078   * Serial callback attaching serial callback to controller object
00079   *
00080   * @param  obj                 PID controller object
00081   * @param  a                   string to read data from
00082   * @param b                   string to write data to
00083   * 
00084   */
00085 void CMotionController::staticSerialCallbackMove(void* obj,char const * a, char * b)
00086 {
00087     CMotionController* self = static_cast<CMotionController*>(obj);
00088     self->serialCallbackMove(a,b);
00089 }
00090 
00091 /** \brief  Serial callback method for BRAKE command
00092   *
00093   * Serial callback attaching serial callback to controller object
00094   *
00095   * @param  obj                 PID controller object
00096   * @param  a                   string to read data from
00097   * @param b                   string to write data to
00098   * 
00099   */
00100 void CMotionController::staticSerialCallbackBrake(void* obj,char const * a, char * b)
00101 {
00102     CMotionController* self = static_cast<CMotionController*>(obj);
00103     self->serialCallbackBrake(a,b);
00104 }
00105 
00106 /** \brief  Serial callback method for hard BRAKE command
00107   *
00108   * Serial callback attaching serial callback to controller object
00109   *
00110   * @param  obj                 PID controller object
00111   * @param  a                   string to read data from
00112   * @param b                   string to write data to
00113   * 
00114   */
00115 void CMotionController::staticSerialCallbackHardBrake(void* obj,char const * a, char * b) 
00116 {
00117     CMotionController* self = static_cast<CMotionController*>(obj);
00118     self->serialCallbackHardBrake(a,b);
00119 }
00120 
00121 /** \brief  Serial callback method for PID activation command
00122   *
00123   * Serial callback attaching serial callback to controller object
00124   *
00125   * @param  obj                 PID controller object
00126   * @param  a                   string to read data from
00127   * @param b                   string to write data to
00128   * 
00129   */
00130 void CMotionController::staticSerialCallbackPID(void* obj,char const * a, char * b) 
00131 {
00132     CMotionController* self = static_cast<CMotionController*>(obj);
00133     self->serialCallbackPID(a,b);
00134 }
00135 
00136 /**
00137  * @brief Static seriel callback for spline command
00138  * 
00139  * @param obj                      object with the serial callback method
00140  * @param a                        string to read data from
00141  * @param b                        string to write data to
00142  */
00143 void CMotionController::staticSerialCallbackSpline(void* obj, char const * a,char * b){
00144     CMotionController* self = static_cast<CMotionController*>(obj);
00145     self->serialCallbackSpline(a,b);
00146 }
00147 
00148 /** \brief  Reset method
00149  * 
00150  *  
00151  *  
00152  */
00153 void CMotionController::reset()
00154 {   
00155     m_speed = 0;
00156     m_angle = 0;
00157 }
00158 
00159 /** \brief  Get speed method
00160  * 
00161  *  
00162  *  \return     Speed
00163  */
00164 float CMotionController::getSpeed() 
00165 {
00166     return m_speed;
00167 }
00168 
00169 /** \brief  Get angle method
00170  * 
00171  *  
00172  *  \return     Angle
00173  */
00174 float CMotionController::getAngle() 
00175 {
00176     return m_angle;
00177 }
00178 
00179 /** \brief  BrakeCallback method
00180  * 
00181  *  
00182  *  
00183  */
00184 void CMotionController::BrakeCallback(){
00185     m_state=2;
00186 }
00187 
00188 /** \brief  Set state method
00189  * 
00190  *  @param  f_state
00191  *  
00192  */
00193 void CMotionController::setState(int f_state){
00194     m_state = f_state;
00195 }
00196 
00197 /** \brief  Method called each f_period
00198  * 
00199  *  
00200  *  
00201  */
00202 void CMotionController::_run()
00203 {
00204     if(m_isSplineActivated) 
00205     {
00206         if(m_motionPlanner.hasValidValue())
00207         {
00208             std::pair<float,float> motion=m_motionPlanner.getNextVelocity();  
00209             float l_dir=m_motionPlanner.getForward()?1:-1;
00210             if(m_ispidActivated)
00211             {
00212                 m_speed=motion.first*l_dir;
00213                 m_angle=motion.second;
00214             }
00215             else
00216             {
00217                 //Pid isn't activated. It have to controllered with the robot speed value in meter per second.  
00218                 m_serialPort.printf("@SPLN:Err1;;\r\n");
00219                 m_state=2;
00220                 m_isSplineActivated=false;
00221                 m_speed=0.0;
00222             }
00223         }
00224         else
00225         {
00226             m_serialPort.printf("@SPLN:Stop;;\r\n");
00227             m_speed=0.0;
00228             m_state=2;
00229             m_isSplineActivated=false;
00230         }
00231     }
00232     
00233     //safety stop function
00234     /**
00235      * @brief It's a part of safety stop functionilty. Check if it's actived or created.
00236      * 
00237      */
00238     if ((m_state!=2)&&(m_safetyStop!=NULL && m_safetyStop->isSafetyStopActive(m_speed,m_angle)==true))
00239     {
00240         m_state = 2;
00241     }
00242 
00243     switch(m_state)
00244     {
00245         // Move
00246         case 1:
00247             m_car.Steer(m_angle);
00248             if(m_ispidActivated && m_control!=NULL)
00249             {
00250                 m_control->setRef(CMotionController::Mps2Rps( m_speed ));
00251                 m_serialPort.printf("%f",CMotionController::Mps2Rps( m_speed ));
00252                 // Calculate control signal
00253                 bool l_isCorrect = m_control->control(); 
00254                 // Check the correct working of the control method
00255                 if( !l_isCorrect ){
00256                     m_car.Brake();
00257                     m_control->clear();
00258                     m_state = 2;
00259                 }
00260                 m_car.Speed(m_control->get()*100.0);//Y
00261             }
00262             else
00263             {
00264                 m_car.Speed(m_speed);
00265             }
00266             break;
00267         // Brake
00268         case 2:
00269             m_car.Steer(m_angle);
00270             m_car.Brake();
00271             if( m_control!=NULL){
00272                 m_control->clear();
00273             }
00274             break;
00275     }
00276     
00277 }
00278 
00279 /** \brief  Serial callback actions for MOVE command
00280   *
00281   * Serial callback method setting controller to values received
00282   *
00283   * @param  a                   string to read data from
00284   * @param b                   string to write data to
00285   * 
00286   */
00287 void CMotionController::serialCallbackMove(char const * a, char * b)
00288 {
00289     float l_speed;
00290     float l_angle;
00291     uint32_t l_res = sscanf(a,"%f;%f",&l_speed,&l_angle);
00292     if (2 == l_res)
00293     {
00294         if( !m_ispidActivated && std::abs(l_speed) > MCTL_PWM_COMMAND_LIMIT ){
00295             sprintf(b,"Command is too high;;");
00296             return;
00297         }if( m_ispidActivated && std::abs(l_speed) > MCTL_SPEED_COMMAND_LIMIT ){
00298             sprintf(b,"Command is too high;;");
00299             return;
00300         }
00301 
00302         m_speed = l_speed;
00303         m_angle = l_angle; 
00304         m_isSplineActivated=false;
00305         m_state=1;
00306         sprintf(b,"ack;;");
00307     }
00308     else
00309     {
00310         sprintf(b,"sintax error;;");
00311     }
00312 }
00313 
00314 /** \brief  Serial callback actions for BRAKE command
00315   *
00316   * Serial callback method setting controller to values received
00317   *
00318   * @param  a                   string to read data from
00319   * @param b                   string to write data to
00320   * 
00321   */
00322 void CMotionController::serialCallbackBrake(char const * a, char * b)
00323 {
00324     float l_angle;
00325     uint32_t l_res = sscanf(a,"%f",&l_angle);
00326     if(1 == l_res)
00327     {
00328         m_speed = 0;
00329         m_angle = l_angle; 
00330         m_state = 2;
00331         if( m_control!=NULL){
00332             m_control->setRef(0);
00333         }
00334         
00335         sprintf(b,"ack;;");           
00336     }
00337     else
00338     {
00339         sprintf(b,"sintax error;;");
00340     }
00341 }
00342 
00343 /** \brief  Serial callback actions for hard BRAKE command
00344   *
00345   * Serial callback method setting controller to values received
00346   *
00347   * @param  a                   string to read data from
00348   * @param b                   string to write data to
00349   * 
00350   */
00351 void CMotionController::serialCallbackHardBrake(char const * a, char * b)
00352 {
00353     float l_brake,l_angle;
00354     uint32_t l_res = sscanf(a,"%f;%f",&l_brake,&l_angle);
00355     if(2 == l_res && m_state!=0)
00356     {
00357         m_speed=0;
00358         m_angle = l_angle; 
00359         m_car.Inverse(l_brake);
00360         m_hbTimeOut.attach(callback(this,&CMotionController::BrakeCallback),0.04);
00361         m_state = 0;
00362         sprintf(b,"ack;;");           
00363     }
00364     else
00365     {
00366         sprintf(b,"sintax error;;");
00367     }
00368 }
00369 
00370 /** \brief  Serial callback actions for hard PID activation command
00371   *
00372   * Serial callback method setting controller to values received
00373   *
00374   * @param  a                   string to read data from
00375   * @param b                   string to write data to
00376   * 
00377   */
00378 void CMotionController::serialCallbackPID(char const * a, char * b)
00379 {
00380     int l_isActivate=0;
00381     uint32_t l_res = sscanf(a,"%d",&l_isActivate);
00382     if(l_res==1)
00383     {   
00384         if(m_control==NULL){
00385             sprintf(b,"Control object wans't instances. Cannot be activate pid controller;;");
00386         }else{
00387             m_ispidActivated=(l_isActivate>=1);
00388             m_state = 2;
00389             sprintf(b,"ack;;");
00390         }
00391         
00392     }else
00393     {
00394         sprintf(b,"sintax error;;");
00395     }
00396 }
00397 
00398 /**
00399  * @brief Serial callback actions for bezier spline command
00400  * 
00401  * @param a                   string to read data from
00402  * @param b                   string to write data to
00403  */
00404 void CMotionController::serialCallbackSpline(char const * a, char * b){
00405     float a_x,a_y,b_x,b_y,c_x,c_y,d_x,d_y,duration_sec;
00406     int isForward=1;
00407     int32_t nrData=sscanf(a,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f",
00408                                     &isForward,
00409                                     &a_x,
00410                                     &a_y,
00411                                     &b_x,
00412                                     &b_y,
00413                                     &c_x,
00414                                     &c_y,
00415                                     &d_x,
00416                                     &d_y,
00417                                     &duration_sec);
00418     if(10==nrData && duration_sec>0 && (isForward==1 || isForward==0)){
00419         m_motionPlanner.setMotionPlannerParameters(static_cast<bool>(isForward), std::complex<float>(a_x,a_y),std::complex<float>(b_x,b_y),std::complex<float>(c_x,c_y),std::complex<float>(d_x,d_y),duration_sec,m_period_sec);
00420         m_isSplineActivated=true;
00421         m_state=1;
00422         sprintf(b,"ack;;");
00423     }else{
00424         sprintf(b,"sintax error;;");
00425     }
00426 }
00427 
00428 
00429 /**
00430  * @brief Function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor.
00431  * 
00432  * @param f_vel_mps linear velocity
00433  * @return float angular velocity
00434  */
00435 float CMotionController::Mps2Rps(float f_vel_mps){
00436     return f_vel_mps * 150.0;
00437 }
00438 
00439 /**
00440  * @brief Static callback function for run method
00441  * 
00442  * @param obj pointer for the object
00443  */
00444 void CMotionController::staticCallbackRun(void* obj){
00445     CMotionController* self = static_cast<CMotionController*>(obj);
00446     self->_run();
00447 }  
00448 
00449 /**
00450  * @brief Start RtosTimer, which periodically apply the run method. The period of the task is defined in the contructor.
00451  * 
00452  */
00453 void CMotionController::startRtosTimer(){
00454     this->m_timer.start(static_cast<int>(m_period_sec*1000));
00455 }