File content as of revision 15:5dfa92698884:

#include "PID.h"

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)
{
    _tick.attach(callback(this, &PID::controlLoop), 0.001);

    _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;

    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) / 54.881;
    _measDistD = ((double)PositionD - (double)oldPositionD) / 54.881;

    // 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 =  _measDistD - _measDistG;

    deltaX = meanDist * cos (_THETA);
    deltaY = meanDist * sin (_THETA);
    deltaTheta = atan2(diffDist,230.0);

    _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;
}