Diff: Controller/controller.cpp

Revision:

9:d5fcdcb3c89d

diff -r 70412939a506 -r d5fcdcb3c89d Controller/controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/controller.cpp	Fri Oct 20 11:41:22 2017 +0000
@@ -0,0 +1,77 @@
+#include "controller.hpp"
+#include "StaticDefs.hpp"
+
+PIDController::PIDController()
+{
+    _integral = 0.0;
+    _lastTime = 0;
+    _loLimit = 0;
+    _hiLimit = 400;
+    _deadbandFlag = false;
+    _deadband = 0.5;
+
+}
+
+void PIDController::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
+    //add a case statement for deadband
+    //add a variable for deadband amount
+    if (_deadbandFlag) {
+        if (abs(_error) < _deadband) {
+            _output = 0.0;
+        }
+    }
+
+}
+
+void PIDController::writeSetPoint(float cmd)
+{
+    _setPoint = cmd;      //<float>(cmd, _loLimit, _hiLimit);
+}
+
+float PIDController::getOutput()
+{
+    return _output;
+}
+
+void PIDController::setPgain(float gain)
+{
+    _Pgain = gain;
+}
+
+void PIDController::setIgain(float gain)
+{
+    _Igain = gain;
+}
+
+void PIDController::setDgain(float gain)
+{
+    _Dgain = gain;
+}
+
+void PIDController::toggleDeadBand(bool toggle)
+{
+    _deadbandFlag = toggle;
+    return;
+}
+void PIDController::setDeadBand(float deadband)
+{
+    _deadband = deadband;
+}
\ No newline at end of file