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@2:d6f14bb935da, 2018-05-22 (annotated)

Committer:
haarkon
Date:
Tue May 22 17:15:51 2018 +0000
Revision:
2:d6f14bb935da
Parent:
1:24c444f3716b
Child:
3:93fb84c4e9bc
added callback treatment of interrupt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:259a31d968a6 1 #include "PID.h"
haarkon 0:259a31d968a6 2
haarkon 1:24c444f3716b 3 PID::PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale, double loopTime) : _encoder (TIM), _pwm(outPWM), _outA(outA), _outB(outB)
haarkon 0:259a31d968a6 4 {
haarkon 0:259a31d968a6 5 Ticker _tick;
haarkon 0:259a31d968a6 6
haarkon 0:259a31d968a6 7 if (loopTime > 0.0001) _loopTime = loopTime;
haarkon 0:259a31d968a6 8 else _loopTime = 0.001;
haarkon 0:259a31d968a6 9 if (scale > 0) _scale = scale ;
haarkon 0:259a31d968a6 10 else _scale = 31.8309886184;
haarkon 0:259a31d968a6 11
haarkon 0:259a31d968a6 12 _pwm.period(1/20000);
haarkon 0:259a31d968a6 13 _pwm = 0;
haarkon 0:259a31d968a6 14
haarkon 0:259a31d968a6 15 _Kp = 1;
haarkon 0:259a31d968a6 16 _Ki = 0;
haarkon 0:259a31d968a6 17 _Kd = 0;
haarkon 0:259a31d968a6 18
haarkon 2:d6f14bb935da 19 _tick.attach(callback(this, &PID::controlLoop), _loopTime);
haarkon 0:259a31d968a6 20 }
haarkon 0:259a31d968a6 21
haarkon 0:259a31d968a6 22 float PID::setProportionnalValue (float KpValue)
haarkon 0:259a31d968a6 23 {
haarkon 0:259a31d968a6 24 _Kp = KpValue;
haarkon 0:259a31d968a6 25 return _Kp;
haarkon 0:259a31d968a6 26 }
haarkon 0:259a31d968a6 27
haarkon 0:259a31d968a6 28 float PID::setintegralValue (float KiValue)
haarkon 0:259a31d968a6 29 {
haarkon 0:259a31d968a6 30 _Ki = KiValue;
haarkon 0:259a31d968a6 31 return _Ki;
haarkon 0:259a31d968a6 32 }
haarkon 0:259a31d968a6 33
haarkon 0:259a31d968a6 34 float PID::setDerivativeValue (float KdValue)
haarkon 0:259a31d968a6 35 {
haarkon 0:259a31d968a6 36 _Kd = KdValue;
haarkon 0:259a31d968a6 37 return _Kd;
haarkon 0:259a31d968a6 38 }
haarkon 0:259a31d968a6 39
haarkon 0:259a31d968a6 40 void PID::setSpeed (float setPoint)
haarkon 0:259a31d968a6 41 {
haarkon 0:259a31d968a6 42 _consigne = setPoint;
haarkon 0:259a31d968a6 43 }
haarkon 0:259a31d968a6 44
haarkon 2:d6f14bb935da 45 long PID::getPosition (void)
haarkon 2:d6f14bb935da 46 {
haarkon 2:d6f14bb935da 47 return _Position;
haarkon 2:d6f14bb935da 48 }
haarkon 2:d6f14bb935da 49
haarkon 0:259a31d968a6 50 void PID::controlLoop()
haarkon 0:259a31d968a6 51 {
haarkon 0:259a31d968a6 52 static int firstTime = 1;
haarkon 0:259a31d968a6 53 static float integralError = 0;
haarkon 0:259a31d968a6 54 static float oldError;
haarkon 0:259a31d968a6 55 double deltaD, error, commande;
haarkon 0:259a31d968a6 56 static long int oldPosition;
haarkon 0:259a31d968a6 57
haarkon 2:d6f14bb935da 58 _Position = _encoder.GetCounter();
haarkon 0:259a31d968a6 59 if (firstTime) {
haarkon 2:d6f14bb935da 60 oldPosition = _Position;
haarkon 0:259a31d968a6 61 firstTime = 0;
haarkon 0:259a31d968a6 62 } else {
haarkon 2:d6f14bb935da 63 deltaD = 1000.0 * ((double)_Position - (double)oldPosition) / _scale;
haarkon 2:d6f14bb935da 64 oldPosition = _Position;
haarkon 0:259a31d968a6 65 error = _consigne - deltaD;
haarkon 0:259a31d968a6 66 integralError += error;
haarkon 0:259a31d968a6 67
haarkon 0:259a31d968a6 68 if (integralError > 10000) {
haarkon 0:259a31d968a6 69 RobotIsStuck = 1;
haarkon 0:259a31d968a6 70 integralError = 10000;
haarkon 0:259a31d968a6 71 }
haarkon 0:259a31d968a6 72 if (integralError < -10000) {
haarkon 0:259a31d968a6 73 RobotIsStuck = 1;
haarkon 0:259a31d968a6 74 integralError = -10000;
haarkon 0:259a31d968a6 75 }
haarkon 0:259a31d968a6 76
haarkon 0:259a31d968a6 77 commande = _Kp * error + _Ki * integralError + _Kd * (error - oldError);
haarkon 0:259a31d968a6 78 oldError = error;
haarkon 0:259a31d968a6 79
haarkon 0:259a31d968a6 80 _PwmValue = commande/1300;
haarkon 0:259a31d968a6 81 if (_PwmValue > 1) _PwmValue = 1;
haarkon 0:259a31d968a6 82 if (_PwmValue < -1) _PwmValue = -1;
haarkon 0:259a31d968a6 83
haarkon 0:259a31d968a6 84 if (_PwmValue < 0) {
haarkon 0:259a31d968a6 85 _pwm = -_PwmValue;
haarkon 0:259a31d968a6 86 _outA = 1;
haarkon 0:259a31d968a6 87 _outB = 0;
haarkon 0:259a31d968a6 88 } else {
haarkon 0:259a31d968a6 89 _pwm = _PwmValue;
haarkon 0:259a31d968a6 90 _outA = 0;
haarkon 0:259a31d968a6 91 _outB = 1;
haarkon 0:259a31d968a6 92 }
haarkon 0:259a31d968a6 93 }
haarkon 0:259a31d968a6 94 }