![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
Diff: PID.cpp
- Revision:
- 0:86c5a1f6e21d
diff -r 000000000000 -r 86c5a1f6e21d PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Tue Jun 05 08:39:37 2018 +0000 @@ -0,0 +1,172 @@ +#include "PID.h" + +PID::PID (TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB, PinName LED1) : _Lencoder(LTIM), _Rencoder(RTIM), _Lpwm(LPWM), _Rpwm(RPWM), _LdirA(LDIRA), _LdirB(LDIRB), _RdirA(RDIRA), _RdirB(RDIRB), _Led1(LED1) +{ + Ticker _tick; + _tick.attach(callback(this, &PID::controlLoop), 0.001); + + _Led1 = 0; + + _Lpwm.period_us(50); + _Lpwm = 0; + + _Rpwm.period_us(50); + _Rpwm = 0; + + _Kp = 2.0; + _Ki = 0.4; + _Kd = 1.0; + +} + +float PID::setProportionnalValue (float KpValue) +{ + _Kp = KpValue; + return _Kp; +} + +float PID::setintegralValue (float KiValue) +{ + _Ki = KiValue; + return _Ki; +} + +float PID::setDerivativeValue (float KdValue) +{ + _Kd = KdValue; + return _Kd; +} + +void PID::setSpeed (double left, double right) +{ + _SpeedG = left; + _SpeedD = right; +} + +void PID::getSpeed (double *vG, double *vD) +{ + *vG = _measSpeedG; + *vD = _measSpeedD; +} + +void PID::getPosition (double *x, double *y, double *theta) +{ + *x = _X; + *y = _Y; + *theta = _THETA; +} + +void PID::resetPosition (void) +{ + _X = 0; + _Y = 0; + _THETA = 0; +} + +void PID::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; + + _Led1 = ! _Led1; + 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 = _measDistG - _measDistD; + + 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; +}