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