29/10/2019
Dependencies: Encoder_Nucleo_16_bits
PID.cpp@2:d6f14bb935da, 2018-05-22 (annotated)
- Committer:
- haarkon
- Date:
- Tue May 22 17:15:51 2018 +0000
- Revision:
- 2:d6f14bb935da
- Parent:
- 1:24c444f3716b
- Child:
- 3:93fb84c4e9bc
added callback treatment of interrupt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:259a31d968a6 | 1 | #include "PID.h" |
haarkon | 0:259a31d968a6 | 2 | |
haarkon | 1:24c444f3716b | 3 | PID::PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale, double loopTime) : _encoder (TIM), _pwm(outPWM), _outA(outA), _outB(outB) |
haarkon | 0:259a31d968a6 | 4 | { |
haarkon | 0:259a31d968a6 | 5 | Ticker _tick; |
haarkon | 0:259a31d968a6 | 6 | |
haarkon | 0:259a31d968a6 | 7 | if (loopTime > 0.0001) _loopTime = loopTime; |
haarkon | 0:259a31d968a6 | 8 | else _loopTime = 0.001; |
haarkon | 0:259a31d968a6 | 9 | if (scale > 0) _scale = scale ; |
haarkon | 0:259a31d968a6 | 10 | else _scale = 31.8309886184; |
haarkon | 0:259a31d968a6 | 11 | |
haarkon | 0:259a31d968a6 | 12 | _pwm.period(1/20000); |
haarkon | 0:259a31d968a6 | 13 | _pwm = 0; |
haarkon | 0:259a31d968a6 | 14 | |
haarkon | 0:259a31d968a6 | 15 | _Kp = 1; |
haarkon | 0:259a31d968a6 | 16 | _Ki = 0; |
haarkon | 0:259a31d968a6 | 17 | _Kd = 0; |
haarkon | 0:259a31d968a6 | 18 | |
haarkon | 2:d6f14bb935da | 19 | _tick.attach(callback(this, &PID::controlLoop), _loopTime); |
haarkon | 0:259a31d968a6 | 20 | } |
haarkon | 0:259a31d968a6 | 21 | |
haarkon | 0:259a31d968a6 | 22 | float PID::setProportionnalValue (float KpValue) |
haarkon | 0:259a31d968a6 | 23 | { |
haarkon | 0:259a31d968a6 | 24 | _Kp = KpValue; |
haarkon | 0:259a31d968a6 | 25 | return _Kp; |
haarkon | 0:259a31d968a6 | 26 | } |
haarkon | 0:259a31d968a6 | 27 | |
haarkon | 0:259a31d968a6 | 28 | float PID::setintegralValue (float KiValue) |
haarkon | 0:259a31d968a6 | 29 | { |
haarkon | 0:259a31d968a6 | 30 | _Ki = KiValue; |
haarkon | 0:259a31d968a6 | 31 | return _Ki; |
haarkon | 0:259a31d968a6 | 32 | } |
haarkon | 0:259a31d968a6 | 33 | |
haarkon | 0:259a31d968a6 | 34 | float PID::setDerivativeValue (float KdValue) |
haarkon | 0:259a31d968a6 | 35 | { |
haarkon | 0:259a31d968a6 | 36 | _Kd = KdValue; |
haarkon | 0:259a31d968a6 | 37 | return _Kd; |
haarkon | 0:259a31d968a6 | 38 | } |
haarkon | 0:259a31d968a6 | 39 | |
haarkon | 0:259a31d968a6 | 40 | void PID::setSpeed (float setPoint) |
haarkon | 0:259a31d968a6 | 41 | { |
haarkon | 0:259a31d968a6 | 42 | _consigne = setPoint; |
haarkon | 0:259a31d968a6 | 43 | } |
haarkon | 0:259a31d968a6 | 44 | |
haarkon | 2:d6f14bb935da | 45 | long PID::getPosition (void) |
haarkon | 2:d6f14bb935da | 46 | { |
haarkon | 2:d6f14bb935da | 47 | return _Position; |
haarkon | 2:d6f14bb935da | 48 | } |
haarkon | 2:d6f14bb935da | 49 | |
haarkon | 0:259a31d968a6 | 50 | void PID::controlLoop() |
haarkon | 0:259a31d968a6 | 51 | { |
haarkon | 0:259a31d968a6 | 52 | static int firstTime = 1; |
haarkon | 0:259a31d968a6 | 53 | static float integralError = 0; |
haarkon | 0:259a31d968a6 | 54 | static float oldError; |
haarkon | 0:259a31d968a6 | 55 | double deltaD, error, commande; |
haarkon | 0:259a31d968a6 | 56 | static long int oldPosition; |
haarkon | 0:259a31d968a6 | 57 | |
haarkon | 2:d6f14bb935da | 58 | _Position = _encoder.GetCounter(); |
haarkon | 0:259a31d968a6 | 59 | if (firstTime) { |
haarkon | 2:d6f14bb935da | 60 | oldPosition = _Position; |
haarkon | 0:259a31d968a6 | 61 | firstTime = 0; |
haarkon | 0:259a31d968a6 | 62 | } else { |
haarkon | 2:d6f14bb935da | 63 | deltaD = 1000.0 * ((double)_Position - (double)oldPosition) / _scale; |
haarkon | 2:d6f14bb935da | 64 | oldPosition = _Position; |
haarkon | 0:259a31d968a6 | 65 | error = _consigne - deltaD; |
haarkon | 0:259a31d968a6 | 66 | integralError += error; |
haarkon | 0:259a31d968a6 | 67 | |
haarkon | 0:259a31d968a6 | 68 | if (integralError > 10000) { |
haarkon | 0:259a31d968a6 | 69 | RobotIsStuck = 1; |
haarkon | 0:259a31d968a6 | 70 | integralError = 10000; |
haarkon | 0:259a31d968a6 | 71 | } |
haarkon | 0:259a31d968a6 | 72 | if (integralError < -10000) { |
haarkon | 0:259a31d968a6 | 73 | RobotIsStuck = 1; |
haarkon | 0:259a31d968a6 | 74 | integralError = -10000; |
haarkon | 0:259a31d968a6 | 75 | } |
haarkon | 0:259a31d968a6 | 76 | |
haarkon | 0:259a31d968a6 | 77 | commande = _Kp * error + _Ki * integralError + _Kd * (error - oldError); |
haarkon | 0:259a31d968a6 | 78 | oldError = error; |
haarkon | 0:259a31d968a6 | 79 | |
haarkon | 0:259a31d968a6 | 80 | _PwmValue = commande/1300; |
haarkon | 0:259a31d968a6 | 81 | if (_PwmValue > 1) _PwmValue = 1; |
haarkon | 0:259a31d968a6 | 82 | if (_PwmValue < -1) _PwmValue = -1; |
haarkon | 0:259a31d968a6 | 83 | |
haarkon | 0:259a31d968a6 | 84 | if (_PwmValue < 0) { |
haarkon | 0:259a31d968a6 | 85 | _pwm = -_PwmValue; |
haarkon | 0:259a31d968a6 | 86 | _outA = 1; |
haarkon | 0:259a31d968a6 | 87 | _outB = 0; |
haarkon | 0:259a31d968a6 | 88 | } else { |
haarkon | 0:259a31d968a6 | 89 | _pwm = _PwmValue; |
haarkon | 0:259a31d968a6 | 90 | _outA = 0; |
haarkon | 0:259a31d968a6 | 91 | _outB = 1; |
haarkon | 0:259a31d968a6 | 92 | } |
haarkon | 0:259a31d968a6 | 93 | } |
haarkon | 0:259a31d968a6 | 94 | } |