File content as of revision 9:d5fcdcb3c89d:

#ifndef MBED_DEPTHCONTROLLER_H
#define MBED_DEPTHCONTROLLER_H

#include "mbed.h"

class PIDController
{
public:
    PIDController();
    
    void update(float position, float velocity, float dt);
    
    float getOutput();
    
    void setPgain(float gain);
    void setIgain(float gain);
    void setDgain(float gain);
    
    void writeSetPoint(float cmd);
    
    void setHiLimit(float limit);
    void setLoLimit(float limit);
    
    void toggleDeadBand(bool toggle); //implement this
    void setDeadBand(float deadband); //implement this
    
protected:
    float _setPoint;
    float _Pgain;
    float _Dgain;
    float _Igain;
    
    float _hiLimit; //these variables clamp the allowable set points 
    float _loLimit; //these variables clamp the allowable set points 
    
    float _error;
    float _deadband;
    bool _deadbandFlag;
    
    float _integral;
    float _lastTime;
    float _dt;
    
       
    float _output;
    
 //   bool configFlag;
 //   int readConfiguration();

};

template <typename T>
T clamp(T value, T min, T max)
{
    if(value < min) {
        return min;
    } else if(value > max) {
        return max;
    } else {
        return value;
    }
};

#endif