File content as of revision 9:d5fcdcb3c89d:

#include "controller.hpp"
#include "StaticDefs.hpp"

PIDController::PIDController()
{
    _integral = 0.0;
    _lastTime = 0;
    _loLimit = 0;
    _hiLimit = 400;
    _deadbandFlag = false;
    _deadband = 0.5;

}

void PIDController::update(float position, float velocity, float dt)
{

    _error = _setPoint - position;

    _integral += (_error*dt);

    _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;

    // limiting on output & integral anti-windup
    if (_output > 1.0) {
        _output = 1.0;
        _integral -= _error*dt;  //implement saturation instead of reset
    } else if (_output  < -1) {
        _output = -1.0;
        _integral += _error*dt;  //implement saturation instead of reset
    }

    //add in some deadband
    //add a case statement for deadband
    //add a variable for deadband amount
    if (_deadbandFlag) {
        if (abs(_error) < _deadband) {
            _output = 0.0;
        }
    }

}

void PIDController::writeSetPoint(float cmd)
{
    _setPoint = cmd;      //<float>(cmd, _loLimit, _hiLimit);
}

float PIDController::getOutput()
{
    return _output;
}

void PIDController::setPgain(float gain)
{
    _Pgain = gain;
}

void PIDController::setIgain(float gain)
{
    _Igain = gain;
}

void PIDController::setDgain(float gain)
{
    _Dgain = gain;
}

void PIDController::toggleDeadBand(bool toggle)
{
    _deadbandFlag = toggle;
    return;
}
void PIDController::setDeadBand(float deadband)
{
    _deadband = deadband;
}