Diff: PID.cpp

Revision:

1:81896d606b4e

Parent:

0:86c5a1f6e21d

--- a/PID.cpp	Tue Jun 05 08:39:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,172 +0,0 @@
-#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;
-}