Diff: PID.cpp

Revision:

0:86c5a1f6e21d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Tue Jun 05 08:39:37 2018 +0000
@@ -0,0 +1,172 @@
+#include "PID.h"
+
+PID::PID (TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB, PinName LED1) : _Lencoder(LTIM), _Rencoder(RTIM), _Lpwm(LPWM), _Rpwm(RPWM), _LdirA(LDIRA), _LdirB(LDIRB), _RdirA(RDIRA), _RdirB(RDIRB), _Led1(LED1)
+{
+    Ticker      _tick;
+    _tick.attach(callback(this, &PID::controlLoop), 0.001);
+
+    _Led1 = 0;
+    
+    _Lpwm.period_us(50);
+    _Lpwm = 0;
+
+    _Rpwm.period_us(50);
+    _Rpwm = 0;
+
+    _Kp = 2.0;
+    _Ki = 0.4;
+    _Kd = 1.0;
+
+}
+
+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 (double left, double right)
+{
+    _SpeedG = left;
+    _SpeedD = right;
+}
+
+void PID::getSpeed (double *vG, double *vD)
+{
+    *vG = _measSpeedG;
+    *vD = _measSpeedD;
+}
+
+void PID::getPosition (double *x, double *y, double *theta)
+{
+    *x = _X;
+    *y = _Y;
+    *theta = _THETA;
+}
+
+void PID::resetPosition (void)
+{
+    _X = 0;
+    _Y = 0;
+    _THETA = 0;
+}
+
+void PID::controlLoop()
+{
+    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;
+
+    _Led1 = ! _Led1;
+    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;
+
+    // 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 {
+        if (integralErrorG < -10000) {
+            _WheelStuckG = 1;
+            integralErrorG = -10000;
+        } else {
+            _WheelStuckG = 0;
+        }
+    }
+
+    if (integralErrorD > 10000) {
+        _WheelStuckD = 1;
+        integralErrorD = 10000;
+    } else {
+        if (integralErrorD < -10000) {
+            _WheelStuckD = 1;
+            integralErrorD = -10000;
+        } else {
+            _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;
+}