Diff: controller.hpp

Revision:

0:2566234da01a

Child:

6:29cf76bb9956

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.hpp	Thu Apr 27 13:16:33 2017 +0000
@@ -0,0 +1,55 @@
+#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 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
\ No newline at end of file