File content as of revision 10:085ab7328054:

#include "PidController.hpp"

PIDController::PIDController() {
    _error = 0.0;
    _integral = 0.0;
    _loLimit = 0.0;
    _hiLimit = 0.0;
    _deadbandFlag = false;
    _deadband = 0.0;
}

void PIDController::update(float position, float velocity, float dt) {
    // error update equations
    _error = _setPoint - position;
    _integral += (_error*dt);

    // pid controller equation
    _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;

    // limiting on output & integral anti-windup
    if (_hiLimit != 0.0 and _output > _hiLimit) {
        _output = _hiLimit;
        _integral -= _error*dt;  // implement saturation
    }
    if (_loLimit != 0.0 and _output < _loLimit) {
        _output = _loLimit;
        _integral += _error*dt;  // implement saturation
    }

    // within deadband on error zeros output
    if (_deadbandFlag) {
        if (abs(_error) < _deadband) {
            _output = 0.0;
        }
    }
}

void PIDController::writeSetPoint(float cmd) {
    _setPoint = cmd;
}

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;
}
void PIDController::setDeadBand(float deadband) {
    _deadband = deadband;
}