FRC - Hackathon / PID

Motion Control library for 2, 3 inputs Full Bridge + Quadrature Encoder, motor system (also called a 2 wheels robot)

Dependencies:   Encoder_Nucleo_16_bits

Dependents:   FRC_2018 FRC2018_Bis 0hackton_08_06_18 lib_FRC_2019 ... more

PID.cpp@13:0227a21c181a, 2019-10-25 (annotated)

Committer:
haarkon
Date:
Fri Oct 25 08:12:41 2019 +0000
Revision:
13:0227a21c181a
Parent:
12:13f42fe66fb1
Child:
15:5dfa92698884
Improved distance measurement

Who changed what in which revision?

UserRevisionLine numberNew 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
haarkon 13:0227a21c181a 78 _measDistG = 20.0*(((double)PositionG - (double)oldPositionG) / 63.662)/23.0;
haarkon 13:0227a21c181a 79 _measDistD = 20.0*(((double)PositionD - (double)oldPositionD) / 63.662)/23.0;
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 5:d01614d14cd1 160 if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
haarkon 5:d01614d14cd1 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 }