test
Dependencies: Encoder_Nucleo_16_bits
PID.cpp@0:259a31d968a6, 2018-05-20 (annotated)
- Committer:
- haarkon
- Date:
- Sun May 20 08:34:23 2018 +0000
- Revision:
- 0:259a31d968a6
- Child:
- 1:24c444f3716b
Untested motion control library for Nucl?o + Quadrature Encoder
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 | 0:259a31d968a6 | 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 | 0:259a31d968a6 | 19 | _tick.attach(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 | 0:259a31d968a6 | 45 | void PID::controlLoop() |
haarkon | 0:259a31d968a6 | 46 | { |
haarkon | 0:259a31d968a6 | 47 | static int firstTime = 1; |
haarkon | 0:259a31d968a6 | 48 | static float integralError = 0; |
haarkon | 0:259a31d968a6 | 49 | static float oldError; |
haarkon | 0:259a31d968a6 | 50 | double deltaD, error, commande; |
haarkon | 0:259a31d968a6 | 51 | long position; |
haarkon | 0:259a31d968a6 | 52 | static long int oldPosition; |
haarkon | 0:259a31d968a6 | 53 | |
haarkon | 0:259a31d968a6 | 54 | position = _encoder.GetCounter(); |
haarkon | 0:259a31d968a6 | 55 | if (firstTime) { |
haarkon | 0:259a31d968a6 | 56 | oldPosition = position; |
haarkon | 0:259a31d968a6 | 57 | firstTime = 0; |
haarkon | 0:259a31d968a6 | 58 | } else { |
haarkon | 0:259a31d968a6 | 59 | deltaD = 1000.0 * ((double)position - (double)oldPosition) / _scale; |
haarkon | 0:259a31d968a6 | 60 | oldPosition = position; |
haarkon | 0:259a31d968a6 | 61 | error = _consigne - deltaD; |
haarkon | 0:259a31d968a6 | 62 | integralError += error; |
haarkon | 0:259a31d968a6 | 63 | |
haarkon | 0:259a31d968a6 | 64 | if (integralError > 10000) { |
haarkon | 0:259a31d968a6 | 65 | RobotIsStuck = 1; |
haarkon | 0:259a31d968a6 | 66 | integralError = 10000; |
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 | |
haarkon | 0:259a31d968a6 | 73 | commande = _Kp * error + _Ki * integralError + _Kd * (error - oldError); |
haarkon | 0:259a31d968a6 | 74 | oldError = error; |
haarkon | 0:259a31d968a6 | 75 | |
haarkon | 0:259a31d968a6 | 76 | _PwmValue = commande/1300; |
haarkon | 0:259a31d968a6 | 77 | if (_PwmValue > 1) _PwmValue = 1; |
haarkon | 0:259a31d968a6 | 78 | if (_PwmValue < -1) _PwmValue = -1; |
haarkon | 0:259a31d968a6 | 79 | |
haarkon | 0:259a31d968a6 | 80 | if (_PwmValue < 0) { |
haarkon | 0:259a31d968a6 | 81 | _pwm = -_PwmValue; |
haarkon | 0:259a31d968a6 | 82 | _outA = 1; |
haarkon | 0:259a31d968a6 | 83 | _outB = 0; |
haarkon | 0:259a31d968a6 | 84 | } else { |
haarkon | 0:259a31d968a6 | 85 | _pwm = _PwmValue; |
haarkon | 0:259a31d968a6 | 86 | _outA = 0; |
haarkon | 0:259a31d968a6 | 87 | _outB = 1; |
haarkon | 0:259a31d968a6 | 88 | } |
haarkon | 0:259a31d968a6 | 89 | } |
haarkon | 0:259a31d968a6 | 90 | } |