File content as of revision 85:dd8176285b6e:

#include "mbed.h"
#include "OuterLoop.hpp"
#include "StaticDefs.hpp"
 
// this is where the variables that can be set are set when the object is created
OuterLoop::OuterLoop(float interval, int sensor):
    _filter(),
    _pid(),
    _pulse()
{
    _Pgain = 0.5;
    _Igain = 0.0;
    _Dgain = 0.1;
    
    _filterFrequency = 2.0;
    _deadband = 0.0;

    _dt = interval;
    _sensor = sensor; // select the sensor ... hacky
    setIHiLimit(3.0);  //give each outerloop instance an integral saturation limit of some default
    setILoLimit(3.0);
    
    if (_sensor == 2) {
        _pid.setHeadingFlag(true);  //used to update the handling for the heading PID control loop
    }
}

// initialize and start all of the member objects.
void OuterLoop::init() {
    // load gains into pid controller this should eventually be in a config file
    setFilterFrequency(_filterFrequency);
    setControllerP(_Pgain);
    setControllerI(_Igain);
    setControllerD(_Dgain);
    
    // setup the controller object
    toggleDeadband(true);
    setDeadband(_deadband);
}

// attaches the update function to an interval timer
void OuterLoop::start() {
    _pulse.attach(this,&OuterLoop::update, _dt);
}
 
// detaches the update function from the interval timer
void OuterLoop::stop() {
    _pulse.detach();
}

void OuterLoop::runOuterLoop() {
    update();
}

// todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this 
void OuterLoop::update() { 
    // update the position velocity filter
    if (_sensor == 0) {
           //   _sensorVal = depth().getDepthFt();  //  change to getting meters
        _sensorVal = depth().getDepthM();
    } else if (_sensor == 1) {
        _sensorVal = imu().getPitch();
    } else if (_sensor == 2) {
        _sensorVal = imu().getHeading();
    } else {
        error("\n\r This sensor option does not exist");
    }
    
    // use the sensor reading to update the PVF
    _filter.update(_dt, _sensorVal);
    
    // refresh the filter results and load into class variables
    refreshPVState();
 
    // update the PID controller with latest data
    _pid.update(_position, _velocity, _filter.getDt());
}

void OuterLoop::setCommand(float value) {
    _SetPoint = value;
    _pid.writeSetPoint(_SetPoint);
}
 
float OuterLoop::getCommand() {
    return _SetPoint;
}

float OuterLoop::getOutput() {
    /* PID output + offset to drive the motors to the correct position */
    return _pid.getOutput() + _offset;
}
 
void OuterLoop::refreshPVState() {
    _position = _filter.getPosition();
    _velocity = _filter.getVelocity();
}
 
float OuterLoop::getPosition() {
    return _position;
}
 
float OuterLoop::getVelocity() {
    return _velocity;
}
 
void OuterLoop::setControllerP(float P) {
    _Pgain = P;
    _pid.setPgain(_Pgain);
}
 
float OuterLoop::getControllerP() {
    return _Pgain;
}
 
void OuterLoop::setControllerI(float I) {
    _Igain = I;
    _pid.setIgain(_Igain);
}
 
float OuterLoop::getControllerI() {
    return _Igain;
}
 
void OuterLoop::setControllerD(float D) {
    _Dgain = D;
    _pid.setDgain(_Dgain);
}
 
float OuterLoop::getControllerD() {
    return _Dgain;
}

//uses position velocity filter class
void OuterLoop::setFilterFrequency(float frequency) {
    _filterFrequency = frequency;
    _filter.writeWn(frequency);   
}

float OuterLoop::getFilterFrequency() {
    return _filterFrequency;               //new 7/11/18
}
 
void OuterLoop::setDeadband(float deadband) {
    _deadband = deadband;
    _pid.setDeadBand(_deadband);
}

float OuterLoop::getDeadband() {
    return _deadband;
}
 
bool OuterLoop::toggleDeadband(bool toggle) {
    _pid.toggleDeadBand(toggle);
    return toggle;
}

void OuterLoop::setOutputOffset(float offset) {
    _offset = offset;
}
 
float OuterLoop::getOutputOffset() {
    return _offset;
}

//not used yet
void OuterLoop::setIHiLimit (float limit){
    _pid.setHiLimit(limit);
}

void OuterLoop::setILoLimit (float limit){
    _pid.setHiLimit(limit);    
}