29/10/2019
Dependencies: Encoder_Nucleo_16_bits
PID.cpp@17:a5af05cade7e, 2019-10-31 (annotated)
- Committer:
- tuk4
- Date:
- Thu Oct 31 14:46:26 2019 +0000
- Revision:
- 17:a5af05cade7e
- Parent:
- 16:a8a0a1e3ddb1
Committed on 31/10/2019
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 | 5:d01614d14cd1 | 3 | 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) |
haarkon | 0:259a31d968a6 | 4 | { |
haarkon | 8:4553677e8b99 | 5 | _tick.attach(callback(this, &PID::controlLoop), 0.001); |
haarkon | 0:259a31d968a6 | 6 | |
haarkon | 5:d01614d14cd1 | 7 | _Lpwm.period_us(50); |
haarkon | 5:d01614d14cd1 | 8 | _Lpwm = 0; |
haarkon | 5:d01614d14cd1 | 9 | |
haarkon | 5:d01614d14cd1 | 10 | _Rpwm.period_us(50); |
haarkon | 5:d01614d14cd1 | 11 | _Rpwm = 0; |
haarkon | 0:259a31d968a6 | 12 | |
haarkon | 5:d01614d14cd1 | 13 | _Kp = 2.0; |
haarkon | 5:d01614d14cd1 | 14 | _Ki = 0.4; |
haarkon | 5:d01614d14cd1 | 15 | _Kd = 1.0; |
haarkon | 0:259a31d968a6 | 16 | } |
haarkon | 0:259a31d968a6 | 17 | |
haarkon | 0:259a31d968a6 | 18 | float PID::setProportionnalValue (float KpValue) |
haarkon | 0:259a31d968a6 | 19 | { |
haarkon | 0:259a31d968a6 | 20 | _Kp = KpValue; |
haarkon | 0:259a31d968a6 | 21 | return _Kp; |
haarkon | 0:259a31d968a6 | 22 | } |
haarkon | 0:259a31d968a6 | 23 | |
haarkon | 0:259a31d968a6 | 24 | float PID::setintegralValue (float KiValue) |
haarkon | 0:259a31d968a6 | 25 | { |
haarkon | 0:259a31d968a6 | 26 | _Ki = KiValue; |
haarkon | 0:259a31d968a6 | 27 | return _Ki; |
haarkon | 0:259a31d968a6 | 28 | } |
haarkon | 0:259a31d968a6 | 29 | |
haarkon | 0:259a31d968a6 | 30 | float PID::setDerivativeValue (float KdValue) |
haarkon | 0:259a31d968a6 | 31 | { |
haarkon | 0:259a31d968a6 | 32 | _Kd = KdValue; |
haarkon | 0:259a31d968a6 | 33 | return _Kd; |
haarkon | 0:259a31d968a6 | 34 | } |
haarkon | 0:259a31d968a6 | 35 | |
haarkon | 5:d01614d14cd1 | 36 | void PID::setSpeed (double left, double right) |
haarkon | 0:259a31d968a6 | 37 | { |
haarkon | 5:d01614d14cd1 | 38 | _SpeedG = left; |
haarkon | 5:d01614d14cd1 | 39 | _SpeedD = right; |
haarkon | 5:d01614d14cd1 | 40 | } |
haarkon | 5:d01614d14cd1 | 41 | |
haarkon | 5:d01614d14cd1 | 42 | void PID::getSpeed (double *vG, double *vD) |
haarkon | 5:d01614d14cd1 | 43 | { |
haarkon | 5:d01614d14cd1 | 44 | *vG = _measSpeedG; |
haarkon | 5:d01614d14cd1 | 45 | *vD = _measSpeedD; |
haarkon | 0:259a31d968a6 | 46 | } |
haarkon | 0:259a31d968a6 | 47 | |
haarkon | 5:d01614d14cd1 | 48 | void PID::getPosition (double *x, double *y, double *theta) |
haarkon | 2:d6f14bb935da | 49 | { |
haarkon | 5:d01614d14cd1 | 50 | *x = _X; |
haarkon | 5:d01614d14cd1 | 51 | *y = _Y; |
haarkon | 5:d01614d14cd1 | 52 | *theta = _THETA; |
haarkon | 5:d01614d14cd1 | 53 | } |
haarkon | 5:d01614d14cd1 | 54 | |
haarkon | 5:d01614d14cd1 | 55 | void PID::resetPosition (void) |
haarkon | 5:d01614d14cd1 | 56 | { |
haarkon | 5:d01614d14cd1 | 57 | _X = 0; |
haarkon | 5:d01614d14cd1 | 58 | _Y = 0; |
haarkon | 5:d01614d14cd1 | 59 | _THETA = 0; |
haarkon | 2:d6f14bb935da | 60 | } |
haarkon | 2:d6f14bb935da | 61 | |
haarkon | 0:259a31d968a6 | 62 | void PID::controlLoop() |
haarkon | 0:259a31d968a6 | 63 | { |
haarkon | 5:d01614d14cd1 | 64 | long PositionG, PositionD; |
haarkon | 5:d01614d14cd1 | 65 | static float integralErrorG = 0, integralErrorD = 0; |
haarkon | 5:d01614d14cd1 | 66 | static float oldErrorG, oldErrorD; |
haarkon | 5:d01614d14cd1 | 67 | double errorG, commandeG; |
haarkon | 5:d01614d14cd1 | 68 | double errorD, commandeD; |
haarkon | 5:d01614d14cd1 | 69 | double meanDist, diffDist, deltaX, deltaY, deltaTheta; |
haarkon | 5:d01614d14cd1 | 70 | static long oldPositionG=0, oldPositionD=0; |
haarkon | 5:d01614d14cd1 | 71 | |
haarkon | 5:d01614d14cd1 | 72 | PositionG = _Lencoder.GetCounter(); |
haarkon | 5:d01614d14cd1 | 73 | PositionD = -_Rencoder.GetCounter(); |
haarkon | 5:d01614d14cd1 | 74 | |
haarkon | 5:d01614d14cd1 | 75 | // As we use mm/s for speed unit and we convert all mesure to this unit |
haarkon | 5:d01614d14cd1 | 76 | |
haarkon | 5:d01614d14cd1 | 77 | // Converting step to millimeters |
tuk4 | 17:a5af05cade7e | 78 | _measDistG = ((double)PositionG - (double)oldPositionG) / 54.881; |
tuk4 | 17:a5af05cade7e | 79 | _measDistD = ((double)PositionD - (double)oldPositionD) / 54.881; |
haarkon | 0:259a31d968a6 | 80 | |
haarkon | 5:d01614d14cd1 | 81 | // Converting position to speed |
haarkon | 5:d01614d14cd1 | 82 | _measSpeedG = 1000.0 * _measDistG; |
haarkon | 5:d01614d14cd1 | 83 | _measSpeedD = 1000.0 * _measDistD; |
haarkon | 5:d01614d14cd1 | 84 | |
haarkon | 5:d01614d14cd1 | 85 | // Computing errors |
haarkon | 5:d01614d14cd1 | 86 | errorG = _SpeedG - _measSpeedG; |
haarkon | 5:d01614d14cd1 | 87 | errorD = _SpeedD - _measSpeedD; |
haarkon | 5:d01614d14cd1 | 88 | integralErrorG += errorG; |
haarkon | 5:d01614d14cd1 | 89 | integralErrorD += errorD; |
haarkon | 5:d01614d14cd1 | 90 | |
haarkon | 5:d01614d14cd1 | 91 | // Limiting integral error |
haarkon | 5:d01614d14cd1 | 92 | if (integralErrorG > 10000) { |
haarkon | 5:d01614d14cd1 | 93 | _WheelStuckG = 1; |
haarkon | 5:d01614d14cd1 | 94 | integralErrorG = 10000; |
haarkon | 0:259a31d968a6 | 95 | } else { |
haarkon | 5:d01614d14cd1 | 96 | if (integralErrorG < -10000) { |
haarkon | 5:d01614d14cd1 | 97 | _WheelStuckG = 1; |
haarkon | 5:d01614d14cd1 | 98 | integralErrorG = -10000; |
haarkon | 5:d01614d14cd1 | 99 | } else { |
haarkon | 5:d01614d14cd1 | 100 | _WheelStuckG = 0; |
haarkon | 0:259a31d968a6 | 101 | } |
haarkon | 5:d01614d14cd1 | 102 | } |
haarkon | 0:259a31d968a6 | 103 | |
haarkon | 5:d01614d14cd1 | 104 | if (integralErrorD > 10000) { |
haarkon | 5:d01614d14cd1 | 105 | _WheelStuckD = 1; |
haarkon | 5:d01614d14cd1 | 106 | integralErrorD = 10000; |
haarkon | 5:d01614d14cd1 | 107 | } else { |
haarkon | 5:d01614d14cd1 | 108 | if (integralErrorD < -10000) { |
haarkon | 5:d01614d14cd1 | 109 | _WheelStuckD = 1; |
haarkon | 5:d01614d14cd1 | 110 | integralErrorD = -10000; |
haarkon | 0:259a31d968a6 | 111 | } else { |
haarkon | 5:d01614d14cd1 | 112 | _WheelStuckD = 0; |
haarkon | 0:259a31d968a6 | 113 | } |
haarkon | 0:259a31d968a6 | 114 | } |
haarkon | 5:d01614d14cd1 | 115 | |
haarkon | 5:d01614d14cd1 | 116 | // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting) |
haarkon | 5:d01614d14cd1 | 117 | commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG); |
haarkon | 5:d01614d14cd1 | 118 | commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD); |
haarkon | 5:d01614d14cd1 | 119 | |
haarkon | 5:d01614d14cd1 | 120 | // Convert to PWM |
haarkon | 5:d01614d14cd1 | 121 | _PwmValueG = commandeG / 1300.0; |
haarkon | 5:d01614d14cd1 | 122 | if (_PwmValueG > 1) _PwmValueG = 1; |
haarkon | 5:d01614d14cd1 | 123 | if (_PwmValueG < -1) _PwmValueG = -1; |
haarkon | 5:d01614d14cd1 | 124 | |
haarkon | 5:d01614d14cd1 | 125 | if (_PwmValueG < 0) { |
haarkon | 5:d01614d14cd1 | 126 | _Lpwm = -_PwmValueG; |
haarkon | 5:d01614d14cd1 | 127 | _LdirA = 0; |
haarkon | 5:d01614d14cd1 | 128 | _LdirB = 1; |
haarkon | 5:d01614d14cd1 | 129 | } else { |
haarkon | 5:d01614d14cd1 | 130 | _Lpwm = _PwmValueG; |
haarkon | 5:d01614d14cd1 | 131 | _LdirA = 1; |
haarkon | 5:d01614d14cd1 | 132 | _LdirB = 0; |
haarkon | 5:d01614d14cd1 | 133 | } |
haarkon | 5:d01614d14cd1 | 134 | |
haarkon | 5:d01614d14cd1 | 135 | _PwmValueD = commandeD/1300.0; |
haarkon | 5:d01614d14cd1 | 136 | if (_PwmValueD > 1) _PwmValueD = 1; |
haarkon | 5:d01614d14cd1 | 137 | if (_PwmValueD < -1) _PwmValueD = -1; |
haarkon | 5:d01614d14cd1 | 138 | |
haarkon | 5:d01614d14cd1 | 139 | if (_PwmValueD < 0) { |
haarkon | 5:d01614d14cd1 | 140 | _Rpwm = -_PwmValueD; |
haarkon | 5:d01614d14cd1 | 141 | _RdirA = 1; |
haarkon | 5:d01614d14cd1 | 142 | _RdirB = 0; |
haarkon | 5:d01614d14cd1 | 143 | } else { |
haarkon | 5:d01614d14cd1 | 144 | _Rpwm = _PwmValueD; |
haarkon | 5:d01614d14cd1 | 145 | _RdirA = 0; |
haarkon | 5:d01614d14cd1 | 146 | _RdirB = 1; |
haarkon | 5:d01614d14cd1 | 147 | } |
haarkon | 5:d01614d14cd1 | 148 | |
haarkon | 5:d01614d14cd1 | 149 | //odometry (segments approx.) |
haarkon | 5:d01614d14cd1 | 150 | meanDist = (_measDistG + _measDistD) / 2.0; |
haarkon | 8:4553677e8b99 | 151 | diffDist = _measDistD - _measDistG; |
haarkon | 5:d01614d14cd1 | 152 | |
haarkon | 5:d01614d14cd1 | 153 | deltaX = meanDist * cos (_THETA); |
haarkon | 5:d01614d14cd1 | 154 | deltaY = meanDist * sin (_THETA); |
haarkon | 12:13f42fe66fb1 | 155 | deltaTheta = atan2(diffDist,230.0); |
haarkon | 5:d01614d14cd1 | 156 | |
haarkon | 5:d01614d14cd1 | 157 | _X += deltaX; |
haarkon | 5:d01614d14cd1 | 158 | _Y += deltaY; |
haarkon | 5:d01614d14cd1 | 159 | _THETA += deltaTheta; |
haarkon | 15:5dfa92698884 | 160 | //if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586; |
haarkon | 15:5dfa92698884 | 161 | //if (_THETA < -3.141592653589793) _THETA += 6.283185307179586; |
haarkon | 5:d01614d14cd1 | 162 | |
haarkon | 5:d01614d14cd1 | 163 | oldPositionG = PositionG; |
haarkon | 5:d01614d14cd1 | 164 | oldPositionD = PositionD; |
haarkon | 5:d01614d14cd1 | 165 | oldErrorG = errorG; |
haarkon | 5:d01614d14cd1 | 166 | oldErrorD = errorD; |
haarkon | 0:259a31d968a6 | 167 | } |