borrowed codes
Dependencies: mbed Encoder_Nucleo_16_bits
main.cpp
- Committer:
- haarkon
- Date:
- 2018-06-06
- Revision:
- 0:242dc469ac2b
- Child:
- 1:e5dd0af74983
File content as of revision 0:242dc469ac2b:
#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); }