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