FRC - Hackathon / Mbed 2 deprecated TestPID

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?

UserRevisionLine numberNew 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 }