File content as of revision 6:29cf76bb9956:

#ifndef MBED_DEPTHCONTROLLER_H
#define MBED_DEPTHCONTROLLER_H

#include "mbed.h"

class PositionController
{
public:
    PositionController();
    
    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 writeSetPoint_la(float cmd);
    
//    void setConfigFlag();
    
protected:
    float _setPoint;
    float _Pgain;
    float _Dgain;
    float _Igain;
    
    float _error;
    
    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