![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
borrowed codes
Dependencies: mbed Encoder_Nucleo_16_bits
Diff: main.cpp
- Revision:
- 0:242dc469ac2b
- Child:
- 1:e5dd0af74983
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 06 20:53:45 2018 +0000 @@ -0,0 +1,299 @@ +#include "mbed.h" +#include "Nucleo_Encoder_16_bits.h" + +#define PI 3.1415926535898 + +Nucleo_Encoder_16_bits _Lencoder (TIM4); +Nucleo_Encoder_16_bits _Rencoder (TIM3); +PwmOut _Lpwm (PA_9); +PwmOut _Rpwm (PA_8); +DigitalOut _LdirA (PC_9); +DigitalOut _LdirB (PC_8); +DigitalOut _RdirA (PC_6); +DigitalOut _RdirB (PC_5); + +Serial pc (PA_2, PA_3, 921600); // Create a serial link to PC for communication + +DigitalOut led1 (PA_5); // Added Led1 for test purpose +DigitalOut led2 (PD_2); // Added Led2 for test purpose +DigitalOut disquette (PA_12); // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed) + +Ticker _tick; + +void controlLoop(void); + +/** + * Set the Kp value + * + * @param Kp (float) : value of Kp + * @return The value of Kp (float) + * @note Can also be accessed using the global variable _Kp + */ +float setProportionnalValue (float KpValue); + +/** + * Set the Ki value + * + * @param Ki (float) : value of Ki + * @return The value of Ki (float) + * @note Can also be accessed using the global variable _Ki + */ +float setintegralValue (float KiValue); + +/** + * Set the Kd value + * + * @param Kd (float) : value of Kd + * @return The value of Kd (float) + * @note Can also be accessed using the global variable _Kd + */ +float setDerivativeValue (float KdValue); + +/** + * Set the Set point value of the speed for integrated full bridge + * + * @param left (double) : Set point value for left wheel speed (in mm/s) + * @param right (double) : Set point value for right wheel speed (in mm/s) + */ +void setSpeed (double left, double right); + +/** + * Get position of the robot (in mm for X and Y and radian for Theta) + * + * @param x (double - passed by reference) : actual position of the center of the robot, in mm, along X axis (front of the robot at startup) + * @param y (double - passed by reference) : actual position of the center of the robot, in mm, along Y axis (left of the robot at startup) + * @param theta (double - passed by reference) : radian angle between the normal to the line passing through the 2 wheels and the angle of the robot at startup + * @note the position is updated each time a motion computation take place (ie : each milliseconds) + */ +void getPosition (double *x, double *y, double *theta); + +/** + * Reset position of the robot (in mm for X and Y and radian for Theta) + * + * @note the positionis equal to 0 on all 3 axis (cannot be undone) + */ +void resetPosition (void); + +/** + * Get speed of the two wheels of the robot + * + * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel + * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel + */ +void getSpeed (double *vG, double *vD); + +/** + * Global Variable of corrective values + * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1 + */ +double _Kp, _Ki, _Kd; +/** + * Global Variable of speed + */ +double _SpeedG, _SpeedD; +/** + * Global Variable of measured speed + */ +double _measSpeedG, _measSpeedD; +/** + * Global Variable of measured displacement + */ +double _measDistG, _measDistD; +/** + * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable) + * @note Must be cleared by user + */ +int _WheelStuckG, _WheelStuckD; +/** + * Global Variable of wheel PWM value + */ +double _PwmValueG, _PwmValueD; +/** + * Global Variable of gravity center of robot position (odometric, error increase with time) + */ +double _X, _Y, _THETA; + +/** + * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable) + * @note Must be cleared by user + */ +int RobotIsStuck; + +int main () +{ + _tick.attach(&controlLoop, 0.001); + + _Lpwm.period_us(50); + _Lpwm = 0; + + _Rpwm.period_us(50); + _Rpwm = 0; + + _Kp = 2.0; + _Ki = 0.4; + _Kd = 1.0; + + pc.printf ("\n\rHelloWorld\n"); + led1 = 1; + led2 = 0; + disquette = 0; + resetPosition(); + wait(3); + + setSpeed (80,80); + while (1); +} + +float setProportionnalValue (float KpValue) +{ + _Kp = KpValue; + return _Kp; +} + +float setintegralValue (float KiValue) +{ + _Ki = KiValue; + return _Ki; +} + +float setDerivativeValue (float KdValue) +{ + _Kd = KdValue; + return _Kd; +} + +void setSpeed (double left, double right) +{ + _SpeedG = left; + _SpeedD = right; +} + +void getSpeed (double *vG, double *vD) +{ + *vG = _measSpeedG; + *vD = _measSpeedD; +} + +void getPosition (double *x, double *y, double *theta) +{ + *x = _X; + *y = _Y; + *theta = _THETA; +} + +void resetPosition (void) +{ + _X = 0; + _Y = 0; + _THETA = 0; +} + +void controlLoop() +{ + long PositionG, PositionD; + static float integralErrorG = 0, integralErrorD = 0; + static float oldErrorG, oldErrorD; + double errorG, commandeG; + double errorD, commandeD; + double meanDist, diffDist, deltaX, deltaY, deltaTheta; + static long oldPositionG=0, oldPositionD=0; + + PositionG = _Lencoder.GetCounter(); + PositionD = -_Rencoder.GetCounter(); + + // As we use mm/s for speed unit and we convert all mesure to this unit + + // Converting step to millimeters + _measDistG = ((double)PositionG - (double)oldPositionG) / 63.662; + _measDistD = ((double)PositionD - (double)oldPositionD) / 63.662; + + // Converting position to speed + _measSpeedG = 1000.0 * _measDistG; + _measSpeedD = 1000.0 * _measDistD; + + // Computing errors + errorG = _SpeedG - _measSpeedG; + errorD = _SpeedD - _measSpeedD; + integralErrorG += errorG; + integralErrorD += errorD; + + // Limiting integral error + if (integralErrorG > 10000) { + _WheelStuckG = 1; + integralErrorG = 10000; + } else { + if (integralErrorG < -10000) { + _WheelStuckG = 1; + integralErrorG = -10000; + } else { + _WheelStuckG = 0; + } + } + + if (integralErrorD > 10000) { + _WheelStuckD = 1; + integralErrorD = 10000; + } else { + if (integralErrorD < -10000) { + _WheelStuckD = 1; + integralErrorD = -10000; + } else { + _WheelStuckD = 0; + } + } + + // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting) + commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG); + commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD); + + // Convert to PWM + _PwmValueG = commandeG / 1300.0; + if (_PwmValueG > 1) _PwmValueG = 1; + if (_PwmValueG < -1) _PwmValueG = -1; + + if (_PwmValueG < 0) { + _Lpwm = -_PwmValueG; + _LdirA = 0; + _LdirB = 1; + } else { + _Lpwm = _PwmValueG; + _LdirA = 1; + _LdirB = 0; + } + + _PwmValueD = commandeD/1300.0; + if (_PwmValueD > 1) _PwmValueD = 1; + if (_PwmValueD < -1) _PwmValueD = -1; + + if (_PwmValueD < 0) { + _Rpwm = -_PwmValueD; + _RdirA = 1; + _RdirB = 0; + } else { + _Rpwm = _PwmValueD; + _RdirA = 0; + _RdirB = 1; + } + + //odometry (segments approx.) + meanDist = (_measDistG + _measDistD) / 2.0; + diffDist = _measDistD - _measDistG; + + deltaX = meanDist * cos (_THETA); + deltaY = meanDist * sin (_THETA); + deltaTheta = diffDist / 230; + + _X += deltaX; + _Y += deltaY; + _THETA += deltaTheta; + if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586; + if (_THETA < -3.141592653589793) _THETA += 6.283185307179586; + + oldPositionG = PositionG; + oldPositionD = PositionD; + oldErrorG = errorG; + oldErrorD = errorD; + + pc.printf ("\rG = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedG, _measDistG, errorG, commandeG, _PwmValueG); + pc.printf ("\rD = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedD, _measDistD, errorD, commandeD, _PwmValueD); +}