Diff: PID.cpp

Revision:

0:259a31d968a6

Child:

1:24c444f3716b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Sun May 20 08:34:23 2018 +0000
@@ -0,0 +1,90 @@
+#include "PID.h"
+
+PID::PID(TIM_TypeDef * _TIM, PinName outPWM, PinName outA, PinName outB, double scale, double loopTime) : _encoder (_TIM), _pwm(outPWM), _outA(outA), _outB(outB)
+{
+    Ticker      _tick;
+
+    if (loopTime > 0.0001)  _loopTime = loopTime;
+    else                    _loopTime = 0.001;
+    if (scale > 0)          _scale = scale ;
+    else                    _scale = 31.8309886184;
+    
+    _pwm.period(1/20000);
+    _pwm = 0;
+
+    _Kp = 1;
+    _Ki = 0;
+    _Kd = 0;
+
+    _tick.attach(this, &PID::controlLoop, _loopTime);
+}
+
+float PID::setProportionnalValue (float KpValue)
+{
+    _Kp = KpValue;
+    return _Kp;
+}
+
+float PID::setintegralValue (float KiValue)
+{
+    _Ki = KiValue;
+    return _Ki;
+}
+
+float PID::setDerivativeValue (float KdValue)
+{
+    _Kd = KdValue;
+    return _Kd;
+}
+
+void PID::setSpeed (float setPoint)
+{
+    _consigne = setPoint;
+}
+
+void PID::controlLoop()
+{
+    static int      firstTime = 1;
+    static float    integralError = 0;
+    static float    oldError;
+    double          deltaD, error, commande;
+    long            position;
+    static long int oldPosition;
+
+    position = _encoder.GetCounter();
+    if (firstTime)  {
+        oldPosition = position;
+        firstTime = 0;
+    } else {
+        deltaD = 1000.0 * ((double)position - (double)oldPosition) / _scale;
+        oldPosition = position;
+        error = _consigne - deltaD;
+        integralError += error;
+
+        if (integralError > 10000) {
+            RobotIsStuck = 1;
+            integralError = 10000;
+        }
+        if (integralError < -10000) {
+            RobotIsStuck = 1;
+            integralError = -10000;
+        }
+
+        commande = _Kp * error + _Ki * integralError + _Kd * (error - oldError);
+        oldError = error;
+
+        _PwmValue = commande/1300;
+        if (_PwmValue >  1) _PwmValue =  1;
+        if (_PwmValue < -1) _PwmValue = -1;
+
+        if (_PwmValue < 0) {
+            _pwm  = -_PwmValue;
+            _outA = 1;
+            _outB = 0;
+        } else {
+            _pwm = _PwmValue;
+            _outA = 0;
+            _outB = 1;
+        }
+    }
+}