Diff: controller.cpp

Revision:

0:2566234da01a

Child:

1:1cc8a8347f22

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