Diff: PID.cpp

Revision:

5:d01614d14cd1

Parent:

3:93fb84c4e9bc

Child:

6:1e8c73d85648

--- a/PID.cpp	Wed May 23 15:10:19 2018 +0000
+++ b/PID.cpp	Tue Jun 05 07:24:40 2018 +0000
@@ -1,22 +1,20 @@
 #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)
+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)
 {
     Ticker      _tick;
 
-    if (loopTime > 0.0001)  _loopTime = loopTime;
-    else                    _loopTime = 0.001;
-    if (scale > 0)          _scale = scale ;
-    else                    _scale = 63.6619772368;
-    
-    _pwm.period(1/20000);
-    _pwm = 0;
+    _Lpwm.period_us(50);
+    _Lpwm = 0;
+
+    _Rpwm.period_us(50);
+    _Rpwm = 0;
 
-    _Kp = 1;
-    _Ki = 0;
-    _Kd = 0;
+    _Kp = 2.0;
+    _Ki = 0.4;
+    _Kd = 1.0;
 
-    _tick.attach(callback(this, &PID::controlLoop), _loopTime);
+    _tick.attach(callback(this, &PID::controlLoop), 0.001);
 }
 
 float PID::setProportionnalValue (float KpValue)
@@ -37,58 +35,135 @@
     return _Kd;
 }
 
-void PID::setSpeed (float setPoint)
+void PID::setSpeed (double left, double right)
 {
-    _consigne = setPoint;
+    _SpeedG = left;
+    _SpeedD = right;
+}
+
+void PID::getSpeed (double *vG, double *vD)
+{
+    *vG = _measSpeedG;
+    *vD = _measSpeedD;
 }
 
-long PID::getPosition (void)
+void PID::getPosition (double *x, double *y, double *theta)
 {
-    return _Position;
+    *x = _X;
+    *y = _Y;
+    *theta = _THETA;
+}
+
+void PID::resetPosition (void)
+{
+    _X = 0;
+    _Y = 0;
+    _THETA = 0;
 }
 
 void PID::controlLoop()
 {
-    static int      firstTime = 1;
-    static float    integralError = 0;
-    static float    oldError;
-    double          deltaD, error, commande;
-    static long int oldPosition;
+    long            PositionG, PositionD;
+    static float    integralErrorG = 0, integralErrorD = 0;
+    static float    oldErrorG, oldErrorD;
+    double          errorG, commandeG;
+    double          errorD, commandeD;
+    double          meanDist, diffDist, deltaX, deltaY, deltaTheta;
+    static long     oldPositionG=0, oldPositionD=0;
+
+    PositionG =  _Lencoder.GetCounter();
+    PositionD = -_Rencoder.GetCounter();
+
+    // As we use mm/s for speed unit and we convert all mesure to this unit
+
+    // Converting step to millimeters
+    _measDistG = ((double)PositionG - (double)oldPositionG) / 63.662;
+    _measDistD = ((double)PositionD - (double)oldPositionD) / 63.662;
 
-    _Position = _encoder.GetCounter();
-    if (firstTime)  {
-        oldPosition = _Position;
-        firstTime = 0;
+    // Converting position to speed
+    _measSpeedG = 1000.0 * _measDistG;
+    _measSpeedD = 1000.0 * _measDistD;
+
+    // Computing errors
+    errorG = _SpeedG - _measSpeedG;
+    errorD = _SpeedD - _measSpeedD;
+    integralErrorG += errorG;
+    integralErrorD += errorD;
+
+    // Limiting integral error
+    if (integralErrorG > 10000) {
+        _WheelStuckG = 1;
+        integralErrorG = 10000;
     } else {
-        deltaD = 1000.0 * ((double)_Position - (double)oldPosition) / _scale;
-        oldPosition = _Position;
-        error = _consigne - deltaD;
-        integralError += error;
-
-        if (integralError > 10000) {
-            RobotIsStuck = 1;
-            integralError = 10000;
+        if (integralErrorG < -10000) {
+            _WheelStuckG = 1;
+            integralErrorG = -10000;
+        } else {
+            _WheelStuckG = 0;
         }
-        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;
+    if (integralErrorD > 10000) {
+        _WheelStuckD = 1;
+        integralErrorD = 10000;
+    } else {
+        if (integralErrorD < -10000) {
+            _WheelStuckD = 1;
+            integralErrorD = -10000;
         } else {
-            _pwm = _PwmValue;
-            _outA = 0;
-            _outB = 1;
+            _WheelStuckD = 0;
         }
     }
+
+    // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting)
+    commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG);
+    commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD);
+
+    // Convert to PWM
+    _PwmValueG = commandeG / 1300.0;
+    if (_PwmValueG >  1) _PwmValueG =  1;
+    if (_PwmValueG < -1) _PwmValueG = -1;
+
+    if (_PwmValueG < 0) {
+        _Lpwm  = -_PwmValueG;
+        _LdirA = 0;
+        _LdirB = 1;
+    } else {
+        _Lpwm = _PwmValueG;
+        _LdirA = 1;
+        _LdirB = 0;
+    }
+
+    _PwmValueD = commandeD/1300.0;
+    if (_PwmValueD >  1) _PwmValueD =  1;
+    if (_PwmValueD < -1) _PwmValueD = -1;
+
+    if (_PwmValueD < 0) {
+        _Rpwm  = -_PwmValueD;
+        _RdirA = 1;
+        _RdirB = 0;
+    } else {
+        _Rpwm = _PwmValueD;
+        _RdirA = 0;
+        _RdirB = 1;
+    }
+
+    //odometry (segments approx.)
+    meanDist = (_measDistG + _measDistD) / 2.0;
+    diffDist =  _measDistG - _measDistD;
+
+    deltaX = meanDist * cos (_THETA);
+    deltaY = meanDist * sin (_THETA);
+    deltaTheta = diffDist / 230;
+
+    _X += deltaX;
+    _Y += deltaY;
+    _THETA += deltaTheta;
+    if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
+    if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;
+
+    oldPositionG = PositionG;
+    oldPositionD = PositionD;
+    oldErrorG = errorG;
+    oldErrorD = errorD;
 }