File content as of revision 92:52a91656458a:

#include "PidController.hpp"

#include "StaticDefs.hpp"

PIDController::PIDController() {
    _error = 0.0;
    _integral = 0.0;
    _loLimit = 0.0;
    _hiLimit = 0.0;
    _deadbandFlag = false;
    _deadband = 0.0;
    _headingFlag = false;   //default for pressure and depth
}

void PIDController::update(float position, float velocity, float dt) {
    // error update equations
    _error = _setPoint - position;
    
    _temp_velocity = velocity;
    
    // hack to get heading update correct
    // need to make sure this makes physical sense
    if (_headingFlag) {
        if (_error >= 180){  //technincally, this adjustment should continue until error is less than 180.
            _error = _error - 360.0;
        }else if(_error <= -180){    // likewise here, continue until error greater than -180
            _error = _error + 360.0;
        }
    }
    
    //calculate integral error term (adding anti-windup)
    // ADDED k_aw or anti-windup term for bench testing (typically very small but anything from 0 to 0.9999...)
    float AWgain = 0.1;    // AntiWindupGain
    float integral_dot = _Pgain * _Igain * (_error - AWgain * abs(_error) * _integral);
    
    //calculate integral error term
    //_integral = _integral + (_error*dt);  //original equation
    _integral = _integral + integral_dot * dt;

    // pid controller equation                                          //derivative error had the wrong sign
    _output = _Pgain*_error + _Igain*_integral - _Dgain*velocity ;      //derivative_error = v_setpoint - v_actual * _Dgain

    // limiting on output & integral anti-windup (preventing integral from getting extremely high)
    // Get the current controller output, and stop integrating if it's saturated    
    // one solution, stop integrating when you the position limits

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

void PIDController::setHeadingFlag(bool heading_flag) {
    _headingFlag = heading_flag;
}

void PIDController::setHiLimit(float high_limit) {
    _hiLimit = high_limit;
}

void PIDController::setLoLimit(float low_limit){
    _loLimit = low_limit;
}