Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

src/MotionController/motioncontroller.cpp@1:ceee5a608e7c, 2019-03-28 (annotated)

Committer:
alexpirciu
Date:
Thu Mar 28 07:44:42 2019 +0000
Revision:
1:ceee5a608e7c
assa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexpirciu 1:ceee5a608e7c 1 /**
alexpirciu 1:ceee5a608e7c 2 ******************************************************************************
alexpirciu 1:ceee5a608e7c 3 * @file MotionController.cpp
alexpirciu 1:ceee5a608e7c 4 * @author RBRO/PJ-IU
alexpirciu 1:ceee5a608e7c 5 * @version V1.0.0
alexpirciu 1:ceee5a608e7c 6 * @date day-month-year
alexpirciu 1:ceee5a608e7c 7 * @brief This file contains the class definition for the motion controller
alexpirciu 1:ceee5a608e7c 8 * functionality.
alexpirciu 1:ceee5a608e7c 9 ******************************************************************************
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11
alexpirciu 1:ceee5a608e7c 12 #include <MotionController/motioncontroller.hpp>
alexpirciu 1:ceee5a608e7c 13
alexpirciu 1:ceee5a608e7c 14
alexpirciu 1:ceee5a608e7c 15 /** \brief CMotionController Class constructor
alexpirciu 1:ceee5a608e7c 16 *
alexpirciu 1:ceee5a608e7c 17 * Constructor method
alexpirciu 1:ceee5a608e7c 18 *
alexpirciu 1:ceee5a608e7c 19 * @param f_period_sec period for controller execution in seconds
alexpirciu 1:ceee5a608e7c 20 * @param f_serialPort reference to serial communication object
alexpirciu 1:ceee5a608e7c 21 * @param f_car reference to MOVE object
alexpirciu 1:ceee5a608e7c 22 * @param f_safetyStop reference to spline safery stop object
alexpirciu 1:ceee5a608e7c 23 * @param f_control reference to controller object
alexpirciu 1:ceee5a608e7c 24 */
alexpirciu 1:ceee5a608e7c 25 CMotionController::CMotionController(
alexpirciu 1:ceee5a608e7c 26 float f_period_sec,
alexpirciu 1:ceee5a608e7c 27 Serial& f_serialPort,
alexpirciu 1:ceee5a608e7c 28 Move& f_car,
alexpirciu 1:ceee5a608e7c 29 CSafetyStopFunction* f_safetyStop,
alexpirciu 1:ceee5a608e7c 30 controllers::CControllerSiso* f_control)
alexpirciu 1:ceee5a608e7c 31 : m_serialPort(f_serialPort)
alexpirciu 1:ceee5a608e7c 32 , m_car(f_car)
alexpirciu 1:ceee5a608e7c 33 , m_speed()
alexpirciu 1:ceee5a608e7c 34 , m_angle()
alexpirciu 1:ceee5a608e7c 35 , m_period_sec(f_period_sec)
alexpirciu 1:ceee5a608e7c 36 , m_isSplineActivated(false)
alexpirciu 1:ceee5a608e7c 37 , m_ispidActivated(false)
alexpirciu 1:ceee5a608e7c 38 , m_motionPlanner()
alexpirciu 1:ceee5a608e7c 39 , m_hbTimeOut()
alexpirciu 1:ceee5a608e7c 40 , m_control(f_control)
alexpirciu 1:ceee5a608e7c 41 , m_safetyStop(f_safetyStop)
alexpirciu 1:ceee5a608e7c 42 , m_timer(mbed::callback(CMotionController::staticCallbackRun,this))
alexpirciu 1:ceee5a608e7c 43 {
alexpirciu 1:ceee5a608e7c 44 }
alexpirciu 1:ceee5a608e7c 45
alexpirciu 1:ceee5a608e7c 46
alexpirciu 1:ceee5a608e7c 47 /** \brief CMotionController Class constructor
alexpirciu 1:ceee5a608e7c 48 *
alexpirciu 1:ceee5a608e7c 49 * Constructor method
alexpirciu 1:ceee5a608e7c 50 *
alexpirciu 1:ceee5a608e7c 51 * @param f_period_sec period for controller execution in seconds
alexpirciu 1:ceee5a608e7c 52 * @param f_serialPort reference to serial communication object
alexpirciu 1:ceee5a608e7c 53 * @param f_car reference to MOVE object
alexpirciu 1:ceee5a608e7c 54 * @param f_control reference to controller object
alexpirciu 1:ceee5a608e7c 55 */
alexpirciu 1:ceee5a608e7c 56 CMotionController::CMotionController(
alexpirciu 1:ceee5a608e7c 57 float f_period_sec,
alexpirciu 1:ceee5a608e7c 58 Serial& f_serialPort,
alexpirciu 1:ceee5a608e7c 59 Move& f_car,
alexpirciu 1:ceee5a608e7c 60 controllers::CControllerSiso* f_control)
alexpirciu 1:ceee5a608e7c 61 : m_serialPort(f_serialPort)
alexpirciu 1:ceee5a608e7c 62 , m_car(f_car)
alexpirciu 1:ceee5a608e7c 63 , m_speed()
alexpirciu 1:ceee5a608e7c 64 , m_angle()
alexpirciu 1:ceee5a608e7c 65 , m_period_sec(f_period_sec)
alexpirciu 1:ceee5a608e7c 66 , m_isSplineActivated(false)
alexpirciu 1:ceee5a608e7c 67 , m_ispidActivated(false)
alexpirciu 1:ceee5a608e7c 68 , m_motionPlanner()
alexpirciu 1:ceee5a608e7c 69 , m_hbTimeOut()
alexpirciu 1:ceee5a608e7c 70 , m_control(f_control)
alexpirciu 1:ceee5a608e7c 71 , m_safetyStop(NULL)
alexpirciu 1:ceee5a608e7c 72 , m_timer(mbed::callback(CMotionController::staticCallbackRun,this))
alexpirciu 1:ceee5a608e7c 73 {
alexpirciu 1:ceee5a608e7c 74 }
alexpirciu 1:ceee5a608e7c 75
alexpirciu 1:ceee5a608e7c 76 /** \brief Serial callback method
alexpirciu 1:ceee5a608e7c 77 *
alexpirciu 1:ceee5a608e7c 78 * Serial callback attaching serial callback to controller object
alexpirciu 1:ceee5a608e7c 79 *
alexpirciu 1:ceee5a608e7c 80 * @param obj PID controller object
alexpirciu 1:ceee5a608e7c 81 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 82 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 83 *
alexpirciu 1:ceee5a608e7c 84 */
alexpirciu 1:ceee5a608e7c 85 void CMotionController::staticSerialCallbackMove(void* obj,char const * a, char * b)
alexpirciu 1:ceee5a608e7c 86 {
alexpirciu 1:ceee5a608e7c 87 CMotionController* self = static_cast<CMotionController*>(obj);
alexpirciu 1:ceee5a608e7c 88 self->serialCallbackMove(a,b);
alexpirciu 1:ceee5a608e7c 89 }
alexpirciu 1:ceee5a608e7c 90
alexpirciu 1:ceee5a608e7c 91 /** \brief Serial callback method for BRAKE command
alexpirciu 1:ceee5a608e7c 92 *
alexpirciu 1:ceee5a608e7c 93 * Serial callback attaching serial callback to controller object
alexpirciu 1:ceee5a608e7c 94 *
alexpirciu 1:ceee5a608e7c 95 * @param obj PID controller object
alexpirciu 1:ceee5a608e7c 96 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 97 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 98 *
alexpirciu 1:ceee5a608e7c 99 */
alexpirciu 1:ceee5a608e7c 100 void CMotionController::staticSerialCallbackBrake(void* obj,char const * a, char * b)
alexpirciu 1:ceee5a608e7c 101 {
alexpirciu 1:ceee5a608e7c 102 CMotionController* self = static_cast<CMotionController*>(obj);
alexpirciu 1:ceee5a608e7c 103 self->serialCallbackBrake(a,b);
alexpirciu 1:ceee5a608e7c 104 }
alexpirciu 1:ceee5a608e7c 105
alexpirciu 1:ceee5a608e7c 106 /** \brief Serial callback method for hard BRAKE command
alexpirciu 1:ceee5a608e7c 107 *
alexpirciu 1:ceee5a608e7c 108 * Serial callback attaching serial callback to controller object
alexpirciu 1:ceee5a608e7c 109 *
alexpirciu 1:ceee5a608e7c 110 * @param obj PID controller object
alexpirciu 1:ceee5a608e7c 111 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 112 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 113 *
alexpirciu 1:ceee5a608e7c 114 */
alexpirciu 1:ceee5a608e7c 115 void CMotionController::staticSerialCallbackHardBrake(void* obj,char const * a, char * b)
alexpirciu 1:ceee5a608e7c 116 {
alexpirciu 1:ceee5a608e7c 117 CMotionController* self = static_cast<CMotionController*>(obj);
alexpirciu 1:ceee5a608e7c 118 self->serialCallbackHardBrake(a,b);
alexpirciu 1:ceee5a608e7c 119 }
alexpirciu 1:ceee5a608e7c 120
alexpirciu 1:ceee5a608e7c 121 /** \brief Serial callback method for PID activation command
alexpirciu 1:ceee5a608e7c 122 *
alexpirciu 1:ceee5a608e7c 123 * Serial callback attaching serial callback to controller object
alexpirciu 1:ceee5a608e7c 124 *
alexpirciu 1:ceee5a608e7c 125 * @param obj PID controller object
alexpirciu 1:ceee5a608e7c 126 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 127 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 128 *
alexpirciu 1:ceee5a608e7c 129 */
alexpirciu 1:ceee5a608e7c 130 void CMotionController::staticSerialCallbackPID(void* obj,char const * a, char * b)
alexpirciu 1:ceee5a608e7c 131 {
alexpirciu 1:ceee5a608e7c 132 CMotionController* self = static_cast<CMotionController*>(obj);
alexpirciu 1:ceee5a608e7c 133 self->serialCallbackPID(a,b);
alexpirciu 1:ceee5a608e7c 134 }
alexpirciu 1:ceee5a608e7c 135
alexpirciu 1:ceee5a608e7c 136 /**
alexpirciu 1:ceee5a608e7c 137 * @brief Static seriel callback for spline command
alexpirciu 1:ceee5a608e7c 138 *
alexpirciu 1:ceee5a608e7c 139 * @param obj object with the serial callback method
alexpirciu 1:ceee5a608e7c 140 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 141 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 142 */
alexpirciu 1:ceee5a608e7c 143 void CMotionController::staticSerialCallbackSpline(void* obj, char const * a,char * b){
alexpirciu 1:ceee5a608e7c 144 CMotionController* self = static_cast<CMotionController*>(obj);
alexpirciu 1:ceee5a608e7c 145 self->serialCallbackSpline(a,b);
alexpirciu 1:ceee5a608e7c 146 }
alexpirciu 1:ceee5a608e7c 147
alexpirciu 1:ceee5a608e7c 148 /** \brief Reset method
alexpirciu 1:ceee5a608e7c 149 *
alexpirciu 1:ceee5a608e7c 150 *
alexpirciu 1:ceee5a608e7c 151 *
alexpirciu 1:ceee5a608e7c 152 */
alexpirciu 1:ceee5a608e7c 153 void CMotionController::reset()
alexpirciu 1:ceee5a608e7c 154 {
alexpirciu 1:ceee5a608e7c 155 m_speed = 0;
alexpirciu 1:ceee5a608e7c 156 m_angle = 0;
alexpirciu 1:ceee5a608e7c 157 }
alexpirciu 1:ceee5a608e7c 158
alexpirciu 1:ceee5a608e7c 159 /** \brief Get speed method
alexpirciu 1:ceee5a608e7c 160 *
alexpirciu 1:ceee5a608e7c 161 *
alexpirciu 1:ceee5a608e7c 162 * \return Speed
alexpirciu 1:ceee5a608e7c 163 */
alexpirciu 1:ceee5a608e7c 164 float CMotionController::getSpeed()
alexpirciu 1:ceee5a608e7c 165 {
alexpirciu 1:ceee5a608e7c 166 return m_speed;
alexpirciu 1:ceee5a608e7c 167 }
alexpirciu 1:ceee5a608e7c 168
alexpirciu 1:ceee5a608e7c 169 /** \brief Get angle method
alexpirciu 1:ceee5a608e7c 170 *
alexpirciu 1:ceee5a608e7c 171 *
alexpirciu 1:ceee5a608e7c 172 * \return Angle
alexpirciu 1:ceee5a608e7c 173 */
alexpirciu 1:ceee5a608e7c 174 float CMotionController::getAngle()
alexpirciu 1:ceee5a608e7c 175 {
alexpirciu 1:ceee5a608e7c 176 return m_angle;
alexpirciu 1:ceee5a608e7c 177 }
alexpirciu 1:ceee5a608e7c 178
alexpirciu 1:ceee5a608e7c 179 /** \brief BrakeCallback method
alexpirciu 1:ceee5a608e7c 180 *
alexpirciu 1:ceee5a608e7c 181 *
alexpirciu 1:ceee5a608e7c 182 *
alexpirciu 1:ceee5a608e7c 183 */
alexpirciu 1:ceee5a608e7c 184 void CMotionController::BrakeCallback(){
alexpirciu 1:ceee5a608e7c 185 m_state=2;
alexpirciu 1:ceee5a608e7c 186 }
alexpirciu 1:ceee5a608e7c 187
alexpirciu 1:ceee5a608e7c 188 /** \brief Set state method
alexpirciu 1:ceee5a608e7c 189 *
alexpirciu 1:ceee5a608e7c 190 * @param f_state
alexpirciu 1:ceee5a608e7c 191 *
alexpirciu 1:ceee5a608e7c 192 */
alexpirciu 1:ceee5a608e7c 193 void CMotionController::setState(int f_state){
alexpirciu 1:ceee5a608e7c 194 m_state = f_state;
alexpirciu 1:ceee5a608e7c 195 }
alexpirciu 1:ceee5a608e7c 196
alexpirciu 1:ceee5a608e7c 197 /** \brief Method called each f_period
alexpirciu 1:ceee5a608e7c 198 *
alexpirciu 1:ceee5a608e7c 199 *
alexpirciu 1:ceee5a608e7c 200 *
alexpirciu 1:ceee5a608e7c 201 */
alexpirciu 1:ceee5a608e7c 202 void CMotionController::_run()
alexpirciu 1:ceee5a608e7c 203 {
alexpirciu 1:ceee5a608e7c 204 if(m_isSplineActivated)
alexpirciu 1:ceee5a608e7c 205 {
alexpirciu 1:ceee5a608e7c 206 if(m_motionPlanner.hasValidValue())
alexpirciu 1:ceee5a608e7c 207 {
alexpirciu 1:ceee5a608e7c 208 std::pair<float,float> motion=m_motionPlanner.getNextVelocity();
alexpirciu 1:ceee5a608e7c 209 float l_dir=m_motionPlanner.getForward()?1:-1;
alexpirciu 1:ceee5a608e7c 210 if(m_ispidActivated)
alexpirciu 1:ceee5a608e7c 211 {
alexpirciu 1:ceee5a608e7c 212 m_speed=motion.first*l_dir;
alexpirciu 1:ceee5a608e7c 213 m_angle=motion.second;
alexpirciu 1:ceee5a608e7c 214 }
alexpirciu 1:ceee5a608e7c 215 else
alexpirciu 1:ceee5a608e7c 216 {
alexpirciu 1:ceee5a608e7c 217 //Pid isn't activated. It have to controllered with the robot speed value in meter per second.
alexpirciu 1:ceee5a608e7c 218 m_serialPort.printf("@SPLN:Err1;;\r\n");
alexpirciu 1:ceee5a608e7c 219 m_state=2;
alexpirciu 1:ceee5a608e7c 220 m_isSplineActivated=false;
alexpirciu 1:ceee5a608e7c 221 m_speed=0.0;
alexpirciu 1:ceee5a608e7c 222 }
alexpirciu 1:ceee5a608e7c 223 }
alexpirciu 1:ceee5a608e7c 224 else
alexpirciu 1:ceee5a608e7c 225 {
alexpirciu 1:ceee5a608e7c 226 m_serialPort.printf("@SPLN:Stop;;\r\n");
alexpirciu 1:ceee5a608e7c 227 m_speed=0.0;
alexpirciu 1:ceee5a608e7c 228 m_state=2;
alexpirciu 1:ceee5a608e7c 229 m_isSplineActivated=false;
alexpirciu 1:ceee5a608e7c 230 }
alexpirciu 1:ceee5a608e7c 231 }
alexpirciu 1:ceee5a608e7c 232
alexpirciu 1:ceee5a608e7c 233 //safety stop function
alexpirciu 1:ceee5a608e7c 234 /**
alexpirciu 1:ceee5a608e7c 235 * @brief It's a part of safety stop functionilty. Check if it's actived or created.
alexpirciu 1:ceee5a608e7c 236 *
alexpirciu 1:ceee5a608e7c 237 */
alexpirciu 1:ceee5a608e7c 238 if ((m_state!=2)&&(m_safetyStop!=NULL && m_safetyStop->isSafetyStopActive(m_speed,m_angle)==true))
alexpirciu 1:ceee5a608e7c 239 {
alexpirciu 1:ceee5a608e7c 240 m_state = 2;
alexpirciu 1:ceee5a608e7c 241 }
alexpirciu 1:ceee5a608e7c 242
alexpirciu 1:ceee5a608e7c 243 switch(m_state)
alexpirciu 1:ceee5a608e7c 244 {
alexpirciu 1:ceee5a608e7c 245 // Move
alexpirciu 1:ceee5a608e7c 246 case 1:
alexpirciu 1:ceee5a608e7c 247 m_car.Steer(m_angle);
alexpirciu 1:ceee5a608e7c 248 if(m_ispidActivated && m_control!=NULL)
alexpirciu 1:ceee5a608e7c 249 {
alexpirciu 1:ceee5a608e7c 250 m_control->setRef(CMotionController::Mps2Rps( m_speed ));
alexpirciu 1:ceee5a608e7c 251 m_serialPort.printf("%f",CMotionController::Mps2Rps( m_speed ));
alexpirciu 1:ceee5a608e7c 252 // Calculate control signal
alexpirciu 1:ceee5a608e7c 253 bool l_isCorrect = m_control->control();
alexpirciu 1:ceee5a608e7c 254 // Check the correct working of the control method
alexpirciu 1:ceee5a608e7c 255 if( !l_isCorrect ){
alexpirciu 1:ceee5a608e7c 256 m_car.Brake();
alexpirciu 1:ceee5a608e7c 257 m_control->clear();
alexpirciu 1:ceee5a608e7c 258 m_state = 2;
alexpirciu 1:ceee5a608e7c 259 }
alexpirciu 1:ceee5a608e7c 260 m_car.Speed(m_control->get()*100.0);//Y
alexpirciu 1:ceee5a608e7c 261 }
alexpirciu 1:ceee5a608e7c 262 else
alexpirciu 1:ceee5a608e7c 263 {
alexpirciu 1:ceee5a608e7c 264 m_car.Speed(m_speed);
alexpirciu 1:ceee5a608e7c 265 }
alexpirciu 1:ceee5a608e7c 266 break;
alexpirciu 1:ceee5a608e7c 267 // Brake
alexpirciu 1:ceee5a608e7c 268 case 2:
alexpirciu 1:ceee5a608e7c 269 m_car.Steer(m_angle);
alexpirciu 1:ceee5a608e7c 270 m_car.Brake();
alexpirciu 1:ceee5a608e7c 271 if( m_control!=NULL){
alexpirciu 1:ceee5a608e7c 272 m_control->clear();
alexpirciu 1:ceee5a608e7c 273 }
alexpirciu 1:ceee5a608e7c 274 break;
alexpirciu 1:ceee5a608e7c 275 }
alexpirciu 1:ceee5a608e7c 276
alexpirciu 1:ceee5a608e7c 277 }
alexpirciu 1:ceee5a608e7c 278
alexpirciu 1:ceee5a608e7c 279 /** \brief Serial callback actions for MOVE command
alexpirciu 1:ceee5a608e7c 280 *
alexpirciu 1:ceee5a608e7c 281 * Serial callback method setting controller to values received
alexpirciu 1:ceee5a608e7c 282 *
alexpirciu 1:ceee5a608e7c 283 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 284 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 285 *
alexpirciu 1:ceee5a608e7c 286 */
alexpirciu 1:ceee5a608e7c 287 void CMotionController::serialCallbackMove(char const * a, char * b)
alexpirciu 1:ceee5a608e7c 288 {
alexpirciu 1:ceee5a608e7c 289 float l_speed;
alexpirciu 1:ceee5a608e7c 290 float l_angle;
alexpirciu 1:ceee5a608e7c 291 uint32_t l_res = sscanf(a,"%f;%f",&l_speed,&l_angle);
alexpirciu 1:ceee5a608e7c 292 if (2 == l_res)
alexpirciu 1:ceee5a608e7c 293 {
alexpirciu 1:ceee5a608e7c 294 if( !m_ispidActivated && std::abs(l_speed) > MCTL_PWM_COMMAND_LIMIT ){
alexpirciu 1:ceee5a608e7c 295 sprintf(b,"Command is too high;;");
alexpirciu 1:ceee5a608e7c 296 return;
alexpirciu 1:ceee5a608e7c 297 }if( m_ispidActivated && std::abs(l_speed) > MCTL_SPEED_COMMAND_LIMIT ){
alexpirciu 1:ceee5a608e7c 298 sprintf(b,"Command is too high;;");
alexpirciu 1:ceee5a608e7c 299 return;
alexpirciu 1:ceee5a608e7c 300 }
alexpirciu 1:ceee5a608e7c 301
alexpirciu 1:ceee5a608e7c 302 m_speed = l_speed;
alexpirciu 1:ceee5a608e7c 303 m_angle = l_angle;
alexpirciu 1:ceee5a608e7c 304 m_isSplineActivated=false;
alexpirciu 1:ceee5a608e7c 305 m_state=1;
alexpirciu 1:ceee5a608e7c 306 sprintf(b,"ack;;");
alexpirciu 1:ceee5a608e7c 307 }
alexpirciu 1:ceee5a608e7c 308 else
alexpirciu 1:ceee5a608e7c 309 {
alexpirciu 1:ceee5a608e7c 310 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 311 }
alexpirciu 1:ceee5a608e7c 312 }
alexpirciu 1:ceee5a608e7c 313
alexpirciu 1:ceee5a608e7c 314 /** \brief Serial callback actions for BRAKE command
alexpirciu 1:ceee5a608e7c 315 *
alexpirciu 1:ceee5a608e7c 316 * Serial callback method setting controller to values received
alexpirciu 1:ceee5a608e7c 317 *
alexpirciu 1:ceee5a608e7c 318 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 319 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 320 *
alexpirciu 1:ceee5a608e7c 321 */
alexpirciu 1:ceee5a608e7c 322 void CMotionController::serialCallbackBrake(char const * a, char * b)
alexpirciu 1:ceee5a608e7c 323 {
alexpirciu 1:ceee5a608e7c 324 float l_angle;
alexpirciu 1:ceee5a608e7c 325 uint32_t l_res = sscanf(a,"%f",&l_angle);
alexpirciu 1:ceee5a608e7c 326 if(1 == l_res)
alexpirciu 1:ceee5a608e7c 327 {
alexpirciu 1:ceee5a608e7c 328 m_speed = 0;
alexpirciu 1:ceee5a608e7c 329 m_angle = l_angle;
alexpirciu 1:ceee5a608e7c 330 m_state = 2;
alexpirciu 1:ceee5a608e7c 331 if( m_control!=NULL){
alexpirciu 1:ceee5a608e7c 332 m_control->setRef(0);
alexpirciu 1:ceee5a608e7c 333 }
alexpirciu 1:ceee5a608e7c 334
alexpirciu 1:ceee5a608e7c 335 sprintf(b,"ack;;");
alexpirciu 1:ceee5a608e7c 336 }
alexpirciu 1:ceee5a608e7c 337 else
alexpirciu 1:ceee5a608e7c 338 {
alexpirciu 1:ceee5a608e7c 339 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 340 }
alexpirciu 1:ceee5a608e7c 341 }
alexpirciu 1:ceee5a608e7c 342
alexpirciu 1:ceee5a608e7c 343 /** \brief Serial callback actions for hard BRAKE command
alexpirciu 1:ceee5a608e7c 344 *
alexpirciu 1:ceee5a608e7c 345 * Serial callback method setting controller to values received
alexpirciu 1:ceee5a608e7c 346 *
alexpirciu 1:ceee5a608e7c 347 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 348 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 349 *
alexpirciu 1:ceee5a608e7c 350 */
alexpirciu 1:ceee5a608e7c 351 void CMotionController::serialCallbackHardBrake(char const * a, char * b)
alexpirciu 1:ceee5a608e7c 352 {
alexpirciu 1:ceee5a608e7c 353 float l_brake,l_angle;
alexpirciu 1:ceee5a608e7c 354 uint32_t l_res = sscanf(a,"%f;%f",&l_brake,&l_angle);
alexpirciu 1:ceee5a608e7c 355 if(2 == l_res && m_state!=0)
alexpirciu 1:ceee5a608e7c 356 {
alexpirciu 1:ceee5a608e7c 357 m_speed=0;
alexpirciu 1:ceee5a608e7c 358 m_angle = l_angle;
alexpirciu 1:ceee5a608e7c 359 m_car.Inverse(l_brake);
alexpirciu 1:ceee5a608e7c 360 m_hbTimeOut.attach(callback(this,&CMotionController::BrakeCallback),0.04);
alexpirciu 1:ceee5a608e7c 361 m_state = 0;
alexpirciu 1:ceee5a608e7c 362 sprintf(b,"ack;;");
alexpirciu 1:ceee5a608e7c 363 }
alexpirciu 1:ceee5a608e7c 364 else
alexpirciu 1:ceee5a608e7c 365 {
alexpirciu 1:ceee5a608e7c 366 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 367 }
alexpirciu 1:ceee5a608e7c 368 }
alexpirciu 1:ceee5a608e7c 369
alexpirciu 1:ceee5a608e7c 370 /** \brief Serial callback actions for hard PID activation command
alexpirciu 1:ceee5a608e7c 371 *
alexpirciu 1:ceee5a608e7c 372 * Serial callback method setting controller to values received
alexpirciu 1:ceee5a608e7c 373 *
alexpirciu 1:ceee5a608e7c 374 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 375 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 376 *
alexpirciu 1:ceee5a608e7c 377 */
alexpirciu 1:ceee5a608e7c 378 void CMotionController::serialCallbackPID(char const * a, char * b)
alexpirciu 1:ceee5a608e7c 379 {
alexpirciu 1:ceee5a608e7c 380 int l_isActivate=0;
alexpirciu 1:ceee5a608e7c 381 uint32_t l_res = sscanf(a,"%d",&l_isActivate);
alexpirciu 1:ceee5a608e7c 382 if(l_res==1)
alexpirciu 1:ceee5a608e7c 383 {
alexpirciu 1:ceee5a608e7c 384 if(m_control==NULL){
alexpirciu 1:ceee5a608e7c 385 sprintf(b,"Control object wans't instances. Cannot be activate pid controller;;");
alexpirciu 1:ceee5a608e7c 386 }else{
alexpirciu 1:ceee5a608e7c 387 m_ispidActivated=(l_isActivate>=1);
alexpirciu 1:ceee5a608e7c 388 m_state = 2;
alexpirciu 1:ceee5a608e7c 389 sprintf(b,"ack;;");
alexpirciu 1:ceee5a608e7c 390 }
alexpirciu 1:ceee5a608e7c 391
alexpirciu 1:ceee5a608e7c 392 }else
alexpirciu 1:ceee5a608e7c 393 {
alexpirciu 1:ceee5a608e7c 394 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 395 }
alexpirciu 1:ceee5a608e7c 396 }
alexpirciu 1:ceee5a608e7c 397
alexpirciu 1:ceee5a608e7c 398 /**
alexpirciu 1:ceee5a608e7c 399 * @brief Serial callback actions for bezier spline command
alexpirciu 1:ceee5a608e7c 400 *
alexpirciu 1:ceee5a608e7c 401 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 402 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 403 */
alexpirciu 1:ceee5a608e7c 404 void CMotionController::serialCallbackSpline(char const * a, char * b){
alexpirciu 1:ceee5a608e7c 405 float a_x,a_y,b_x,b_y,c_x,c_y,d_x,d_y,duration_sec;
alexpirciu 1:ceee5a608e7c 406 int isForward=1;
alexpirciu 1:ceee5a608e7c 407 int32_t nrData=sscanf(a,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f",
alexpirciu 1:ceee5a608e7c 408 &isForward,
alexpirciu 1:ceee5a608e7c 409 &a_x,
alexpirciu 1:ceee5a608e7c 410 &a_y,
alexpirciu 1:ceee5a608e7c 411 &b_x,
alexpirciu 1:ceee5a608e7c 412 &b_y,
alexpirciu 1:ceee5a608e7c 413 &c_x,
alexpirciu 1:ceee5a608e7c 414 &c_y,
alexpirciu 1:ceee5a608e7c 415 &d_x,
alexpirciu 1:ceee5a608e7c 416 &d_y,
alexpirciu 1:ceee5a608e7c 417 &duration_sec);
alexpirciu 1:ceee5a608e7c 418 if(10==nrData && duration_sec>0 && (isForward==1 || isForward==0)){
alexpirciu 1:ceee5a608e7c 419 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);
alexpirciu 1:ceee5a608e7c 420 m_isSplineActivated=true;
alexpirciu 1:ceee5a608e7c 421 m_state=1;
alexpirciu 1:ceee5a608e7c 422 sprintf(b,"ack;;");
alexpirciu 1:ceee5a608e7c 423 }else{
alexpirciu 1:ceee5a608e7c 424 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 425 }
alexpirciu 1:ceee5a608e7c 426 }
alexpirciu 1:ceee5a608e7c 427
alexpirciu 1:ceee5a608e7c 428
alexpirciu 1:ceee5a608e7c 429 /**
alexpirciu 1:ceee5a608e7c 430 * @brief Function to convert from linear velocity ( centimeter per second ) of robot to angular velocity ( rotation per second ) of motor.
alexpirciu 1:ceee5a608e7c 431 *
alexpirciu 1:ceee5a608e7c 432 * @param f_vel_mps linear velocity
alexpirciu 1:ceee5a608e7c 433 * @return float angular velocity
alexpirciu 1:ceee5a608e7c 434 */
alexpirciu 1:ceee5a608e7c 435 float CMotionController::Mps2Rps(float f_vel_mps){
alexpirciu 1:ceee5a608e7c 436 return f_vel_mps * 150.0;
alexpirciu 1:ceee5a608e7c 437 }
alexpirciu 1:ceee5a608e7c 438
alexpirciu 1:ceee5a608e7c 439 /**
alexpirciu 1:ceee5a608e7c 440 * @brief Static callback function for run method
alexpirciu 1:ceee5a608e7c 441 *
alexpirciu 1:ceee5a608e7c 442 * @param obj pointer for the object
alexpirciu 1:ceee5a608e7c 443 */
alexpirciu 1:ceee5a608e7c 444 void CMotionController::staticCallbackRun(void* obj){
alexpirciu 1:ceee5a608e7c 445 CMotionController* self = static_cast<CMotionController*>(obj);
alexpirciu 1:ceee5a608e7c 446 self->_run();
alexpirciu 1:ceee5a608e7c 447 }
alexpirciu 1:ceee5a608e7c 448
alexpirciu 1:ceee5a608e7c 449 /**
alexpirciu 1:ceee5a608e7c 450 * @brief Start RtosTimer, which periodically apply the run method. The period of the task is defined in the contructor.
alexpirciu 1:ceee5a608e7c 451 *
alexpirciu 1:ceee5a608e7c 452 */
alexpirciu 1:ceee5a608e7c 453 void CMotionController::startRtosTimer(){
alexpirciu 1:ceee5a608e7c 454 this->m_timer.start(static_cast<int>(m_period_sec*1000));
alexpirciu 1:ceee5a608e7c 455 }