test
Dependencies: Encoder_Nucleo_16_bits
Diff: PID.cpp
- Revision:
- 0:259a31d968a6
- Child:
- 1:24c444f3716b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Sun May 20 08:34:23 2018 +0000 @@ -0,0 +1,90 @@ +#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) +{ + Ticker _tick; + + if (loopTime > 0.0001) _loopTime = loopTime; + else _loopTime = 0.001; + if (scale > 0) _scale = scale ; + else _scale = 31.8309886184; + + _pwm.period(1/20000); + _pwm = 0; + + _Kp = 1; + _Ki = 0; + _Kd = 0; + + _tick.attach(this, &PID::controlLoop, _loopTime); +} + +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 (float setPoint) +{ + _consigne = setPoint; +} + +void PID::controlLoop() +{ + static int firstTime = 1; + static float integralError = 0; + static float oldError; + double deltaD, error, commande; + long position; + static long int oldPosition; + + position = _encoder.GetCounter(); + if (firstTime) { + oldPosition = position; + firstTime = 0; + } else { + deltaD = 1000.0 * ((double)position - (double)oldPosition) / _scale; + oldPosition = position; + error = _consigne - deltaD; + integralError += error; + + if (integralError > 10000) { + RobotIsStuck = 1; + integralError = 10000; + } + 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; + } else { + _pwm = _PwmValue; + _outA = 0; + _outB = 1; + } + } +}