Diff: PidController/PidController.cpp

Revision:

10:085ab7328054

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidController/PidController.cpp	Mon Oct 23 12:50:53 2017 +0000
@@ -0,0 +1,63 @@
+#include "PidController.hpp"
+
+PIDController::PIDController() {
+    _error = 0.0;
+    _integral = 0.0;
+    _loLimit = 0.0;
+    _hiLimit = 0.0;
+    _deadbandFlag = false;
+    _deadband = 0.0;
+}
+
+void PIDController::update(float position, float velocity, float dt) {
+    // error update equations
+    _error = _setPoint - position;
+    _integral += (_error*dt);
+
+    // pid controller equation
+    _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ;
+
+    // limiting on output & integral anti-windup
+    if (_hiLimit != 0.0 and _output > _hiLimit) {
+        _output = _hiLimit;
+        _integral -= _error*dt;  // implement saturation
+    }
+    if (_loLimit != 0.0 and _output < _loLimit) {
+        _output = _loLimit;
+        _integral += _error*dt;  // implement saturation
+    }
+
+    // within deadband on error zeros output
+    if (_deadbandFlag) {
+        if (abs(_error) < _deadband) {
+            _output = 0.0;
+        }
+    }
+}
+
+void PIDController::writeSetPoint(float cmd) {
+    _setPoint = cmd;
+}
+
+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;
+}
+void PIDController::setDeadBand(float deadband) {
+    _deadband = deadband;
+}
\ No newline at end of file