test
Dependencies: Encoder_Nucleo_16_bits
Diff: PID.cpp
- Revision:
- 5:d01614d14cd1
- Parent:
- 3:93fb84c4e9bc
- Child:
- 6:1e8c73d85648
diff -r 5dea450cbc80 -r d01614d14cd1 PID.cpp --- a/PID.cpp Wed May 23 15:10:19 2018 +0000 +++ b/PID.cpp Tue Jun 05 07:24:40 2018 +0000 @@ -1,22 +1,20 @@ #include "PID.h" -PID::PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale, double loopTime) : _encoder (TIM), _pwm(outPWM), _outA(outA), _outB(outB) +PID::PID (TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB) : _Lencoder(LTIM), _Rencoder(RTIM), _Lpwm(LPWM), _Rpwm(RPWM), _LdirA(LDIRA), _LdirB(LDIRB), _RdirA(RDIRA), _RdirB(RDIRB) { Ticker _tick; - if (loopTime > 0.0001) _loopTime = loopTime; - else _loopTime = 0.001; - if (scale > 0) _scale = scale ; - else _scale = 63.6619772368; - - _pwm.period(1/20000); - _pwm = 0; + _Lpwm.period_us(50); + _Lpwm = 0; + + _Rpwm.period_us(50); + _Rpwm = 0; - _Kp = 1; - _Ki = 0; - _Kd = 0; + _Kp = 2.0; + _Ki = 0.4; + _Kd = 1.0; - _tick.attach(callback(this, &PID::controlLoop), _loopTime); + _tick.attach(callback(this, &PID::controlLoop), 0.001); } float PID::setProportionnalValue (float KpValue) @@ -37,58 +35,135 @@ return _Kd; } -void PID::setSpeed (float setPoint) +void PID::setSpeed (double left, double right) { - _consigne = setPoint; + _SpeedG = left; + _SpeedD = right; +} + +void PID::getSpeed (double *vG, double *vD) +{ + *vG = _measSpeedG; + *vD = _measSpeedD; } -long PID::getPosition (void) +void PID::getPosition (double *x, double *y, double *theta) { - return _Position; + *x = _X; + *y = _Y; + *theta = _THETA; +} + +void PID::resetPosition (void) +{ + _X = 0; + _Y = 0; + _THETA = 0; } void PID::controlLoop() { - static int firstTime = 1; - static float integralError = 0; - static float oldError; - double deltaD, error, commande; - static long int oldPosition; + 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; - _Position = _encoder.GetCounter(); - if (firstTime) { - oldPosition = _Position; - firstTime = 0; + // 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 { - deltaD = 1000.0 * ((double)_Position - (double)oldPosition) / _scale; - oldPosition = _Position; - error = _consigne - deltaD; - integralError += error; - - if (integralError > 10000) { - RobotIsStuck = 1; - integralError = 10000; + if (integralErrorG < -10000) { + _WheelStuckG = 1; + integralErrorG = -10000; + } else { + _WheelStuckG = 0; } - if (integralError < -10000) { - RobotIsStuck = 1; - integralError = -10000; - } - - commande = _Kp * error + _Ki * integralError + _Kd * (error - oldError); - oldError = error; + } - _PwmValue = commande/1300; - if (_PwmValue > 1) _PwmValue = 1; - if (_PwmValue < -1) _PwmValue = -1; - - if (_PwmValue < 0) { - _pwm = -_PwmValue; - _outA = 1; - _outB = 0; + if (integralErrorD > 10000) { + _WheelStuckD = 1; + integralErrorD = 10000; + } else { + if (integralErrorD < -10000) { + _WheelStuckD = 1; + integralErrorD = -10000; } else { - _pwm = _PwmValue; - _outA = 0; - _outB = 1; + _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; }