![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
a
src/MotionController/motioncontroller.cpp
- Committer:
- alexpirciu
- Date:
- 2019-03-28
- Revision:
- 1:ceee5a608e7c
File content as of revision 1:ceee5a608e7c:
/** ****************************************************************************** * @file MotionController.cpp * @author RBRO/PJ-IU * @version V1.0.0 * @date day-month-year * @brief This file contains the class definition for the motion controller * functionality. ****************************************************************************** */ #include <MotionController/motioncontroller.hpp> /** \brief CMotionController Class constructor * * Constructor method * * @param f_period_sec period for controller execution in seconds * @param f_serialPort reference to serial communication object * @param f_car reference to MOVE object * @param f_safetyStop reference to spline safery stop object * @param f_control reference to controller object */ CMotionController::CMotionController( float f_period_sec, Serial& f_serialPort, Move& f_car, CSafetyStopFunction* f_safetyStop, controllers::CControllerSiso* f_control) : m_serialPort(f_serialPort) , m_car(f_car) , m_speed() , m_angle() , m_period_sec(f_period_sec) , m_isSplineActivated(false) , m_ispidActivated(false) , m_motionPlanner() , m_hbTimeOut() , m_control(f_control) , m_safetyStop(f_safetyStop) , m_timer(mbed::callback(CMotionController::staticCallbackRun,this)) { } /** \brief CMotionController Class constructor * * Constructor method * * @param f_period_sec period for controller execution in seconds * @param f_serialPort reference to serial communication object * @param f_car reference to MOVE object * @param f_control reference to controller object */ CMotionController::CMotionController( float f_period_sec, Serial& f_serialPort, Move& f_car, controllers::CControllerSiso* f_control) : m_serialPort(f_serialPort) , m_car(f_car) , m_speed() , m_angle() , m_period_sec(f_period_sec) , m_isSplineActivated(false) , m_ispidActivated(false) , m_motionPlanner() , m_hbTimeOut() , m_control(f_control) , m_safetyStop(NULL) , m_timer(mbed::callback(CMotionController::staticCallbackRun,this)) { } /** \brief Serial callback method * * Serial callback attaching serial callback to controller object * * @param obj PID controller object * @param a string to read data from * @param b string to write data to * */ void CMotionController::staticSerialCallbackMove(void* obj,char const * a, char * b) { CMotionController* self = static_cast<CMotionController*>(obj); self->serialCallbackMove(a,b); } /** \brief Serial callback method for BRAKE command * * Serial callback attaching serial callback to controller object * * @param obj PID controller object * @param a string to read data from * @param b string to write data to * */ void CMotionController::staticSerialCallbackBrake(void* obj,char const * a, char * b) { CMotionController* self = static_cast<CMotionController*>(obj); self->serialCallbackBrake(a,b); } /** \brief Serial callback method for hard BRAKE command * * Serial callback attaching serial callback to controller object * * @param obj PID controller object * @param a string to read data from * @param b string to write data to * */ void CMotionController::staticSerialCallbackHardBrake(void* obj,char const * a, char * b) { CMotionController* self = static_cast<CMotionController*>(obj); self->serialCallbackHardBrake(a,b); } /** \brief Serial callback method for PID activation command * * Serial callback attaching serial callback to controller object * * @param obj PID controller object * @param a string to read data from * @param b string to write data to * */ void CMotionController::staticSerialCallbackPID(void* obj,char const * a, char * b) { CMotionController* self = static_cast<CMotionController*>(obj); self->serialCallbackPID(a,b); } /** * @brief Static seriel callback for spline command * * @param obj object with the serial callback method * @param a string to read data from * @param b string to write data to */ void CMotionController::staticSerialCallbackSpline(void* obj, char const * a,char * b){ CMotionController* self = static_cast<CMotionController*>(obj); self->serialCallbackSpline(a,b); } /** \brief Reset method * * * */ void CMotionController::reset() { m_speed = 0; m_angle = 0; } /** \brief Get speed method * * * \return Speed */ float CMotionController::getSpeed() { return m_speed; } /** \brief Get angle method * * * \return Angle */ float CMotionController::getAngle() { return m_angle; } /** \brief BrakeCallback method * * * */ void CMotionController::BrakeCallback(){ m_state=2; } /** \brief Set state method * * @param f_state * */ void CMotionController::setState(int f_state){ m_state = f_state; } /** \brief Method called each f_period * * * */ void CMotionController::_run() { if(m_isSplineActivated) { if(m_motionPlanner.hasValidValue()) { std::pair<float,float> motion=m_motionPlanner.getNextVelocity(); float l_dir=m_motionPlanner.getForward()?1:-1; if(m_ispidActivated) { m_speed=motion.first*l_dir; m_angle=motion.second; } else { //Pid isn't activated. It have to controllered with the robot speed value in meter per second. m_serialPort.printf("@SPLN:Err1;;\r\n"); m_state=2; m_isSplineActivated=false; m_speed=0.0; } } else { m_serialPort.printf("@SPLN:Stop;;\r\n"); m_speed=0.0; m_state=2; m_isSplineActivated=false; } } //safety stop function /** * @brief It's a part of safety stop functionilty. Check if it's actived or created. * */ if ((m_state!=2)&&(m_safetyStop!=NULL && m_safetyStop->isSafetyStopActive(m_speed,m_angle)==true)) { m_state = 2; } switch(m_state) { // Move case 1: m_car.Steer(m_angle); if(m_ispidActivated && m_control!=NULL) { m_control->setRef(CMotionController::Mps2Rps( m_speed )); m_serialPort.printf("%f",CMotionController::Mps2Rps( m_speed )); // Calculate control signal bool l_isCorrect = m_control->control(); // Check the correct working of the control method if( !l_isCorrect ){ m_car.Brake(); m_control->clear(); m_state = 2; } m_car.Speed(m_control->get()*100.0);//Y } else { m_car.Speed(m_speed); } break; // Brake case 2: m_car.Steer(m_angle); m_car.Brake(); if( m_control!=NULL){ m_control->clear(); } break; } } /** \brief Serial callback actions for MOVE command * * Serial callback method setting controller to values received * * @param a string to read data from * @param b string to write data to * */ void CMotionController::serialCallbackMove(char const * a, char * b) { float l_speed; float l_angle; uint32_t l_res = sscanf(a,"%f;%f",&l_speed,&l_angle); if (2 == l_res) { if( !m_ispidActivated && std::abs(l_speed) > MCTL_PWM_COMMAND_LIMIT ){ sprintf(b,"Command is too high;;"); return; }if( m_ispidActivated && std::abs(l_speed) > MCTL_SPEED_COMMAND_LIMIT ){ sprintf(b,"Command is too high;;"); return; } m_speed = l_speed; m_angle = l_angle; m_isSplineActivated=false; m_state=1; sprintf(b,"ack;;"); } else { sprintf(b,"sintax error;;"); } } /** \brief Serial callback actions for BRAKE command * * Serial callback method setting controller to values received * * @param a string to read data from * @param b string to write data to * */ void CMotionController::serialCallbackBrake(char const * a, char * b) { float l_angle; uint32_t l_res = sscanf(a,"%f",&l_angle); if(1 == l_res) { m_speed = 0; m_angle = l_angle; m_state = 2; if( m_control!=NULL){ m_control->setRef(0); } sprintf(b,"ack;;"); } else { sprintf(b,"sintax error;;"); } } /** \brief Serial callback actions for hard BRAKE command * * Serial callback method setting controller to values received * * @param a string to read data from * @param b string to write data to * */ void CMotionController::serialCallbackHardBrake(char const * a, char * b) { float l_brake,l_angle; uint32_t l_res = sscanf(a,"%f;%f",&l_brake,&l_angle); if(2 == l_res && m_state!=0) { m_speed=0; m_angle = l_angle; m_car.Inverse(l_brake); m_hbTimeOut.attach(callback(this,&CMotionController::BrakeCallback),0.04); m_state = 0; sprintf(b,"ack;;"); } else { sprintf(b,"sintax error;;"); } } /** \brief Serial callback actions for hard PID activation command * * Serial callback method setting controller to values received * * @param a string to read data from * @param b string to write data to * */ void CMotionController::serialCallbackPID(char const * a, char * b) { int l_isActivate=0; uint32_t l_res = sscanf(a,"%d",&l_isActivate); if(l_res==1) { if(m_control==NULL){ sprintf(b,"Control object wans't instances. Cannot be activate pid controller;;"); }else{ m_ispidActivated=(l_isActivate>=1); m_state = 2; sprintf(b,"ack;;"); } }else { sprintf(b,"sintax error;;"); } } /** * @brief Serial callback actions for bezier spline command * * @param a string to read data from * @param b string to write data to */ void CMotionController::serialCallbackSpline(char const * a, char * b){ float a_x,a_y,b_x,b_y,c_x,c_y,d_x,d_y,duration_sec; int isForward=1; int32_t nrData=sscanf(a,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f", &isForward, &a_x, &a_y, &b_x, &b_y, &c_x, &c_y, &d_x, &d_y, &duration_sec); if(10==nrData && duration_sec>0 && (isForward==1 || isForward==0)){ 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); m_isSplineActivated=true; m_state=1; sprintf(b,"ack;;"); }else{ sprintf(b,"sintax error;;"); } } /** * @brief Function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor. * * @param f_vel_mps linear velocity * @return float angular velocity */ float CMotionController::Mps2Rps(float f_vel_mps){ return f_vel_mps * 150.0; } /** * @brief Static callback function for run method * * @param obj pointer for the object */ void CMotionController::staticCallbackRun(void* obj){ CMotionController* self = static_cast<CMotionController*>(obj); self->_run(); } /** * @brief Start RtosTimer, which periodically apply the run method. The period of the task is defined in the contructor. * */ void CMotionController::startRtosTimer(){ this->m_timer.start(static_cast<int>(m_period_sec*1000)); }