File content as of revision 3:c381d9b58867:

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

PositionController::PositionController()
{
    _integral = 0.0;
    _lastTime = 0;

}

void PositionController::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
    if (_error < 0.5 && _error > -0.5) {
        _output = 0.0;
    }
}

void PositionController::writeSetPoint(float cmd)
{
    //33mm and 400mm are the safe allowable limits that the piston can go to
    _setPoint = clamp<float>(cmd, 0.0, 500.0); //changed 400 to 500 on 6/5/17 //33 to 50
}

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

/*int DepthController::readConfiguration()
{
    ConfigFile cfg;
    int count = 0;
    cfg.read("/local/gains.txt");
    if (cfg.getValue("PDepth", &Pgain , 0.0)) {
        count++;
    }
    if (cfg.getValue("IDepth", &Igain , 0.0)) {
        count++;
    }
    if (cfg.getValue("DDepth", &Dgain , 0.0)) {
        count++;
    }
    //uncomment the following to debug config file issues
    //pc().printf("Pgain:%2.2f  Igain:%2.2f  Dgain:%2.2f", Pgain, Igain, Dgain);

    return count;
} */

//void DepthController::setConfigFlag()
//{
//   configFlag = true;
//}

void PositionController::setPgain(float gain)
{
    _Pgain = gain;
    pc().printf("\n\rPgain Depth Controller = %f\n\r", _Pgain);
}

void PositionController::setIgain(float gain)
{
    _Igain = gain;
    pc().printf("\n\rIgain Depth Controller = %f\n\r", _Igain);
}

void PositionController::setDgain(float gain)
{
    _Dgain = gain;
    pc().printf("\n\rDgain Depth Controller = %f\n\r", _Dgain);
}