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@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?

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