FRC - Hackathon / PID

Motion Control library for 2, 3 inputs Full Bridge + Quadrature Encoder, motor system (also called a 2 wheels robot)

Dependencies:   Encoder_Nucleo_16_bits

Dependents:   FRC_2018 FRC2018_Bis 0hackton_08_06_18 lib_FRC_2019 ... more

PID.cpp

Committer:
haarkon
Date:
2018-05-20
Revision:
0:259a31d968a6
Child:
1:24c444f3716b

File content as of revision 0:259a31d968a6:

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