Diff: PidController/PidController.cpp

Revision:

59:4c04d5c7aed1

Child:

92:52a91656458a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidController/PidController.cpp	Fri Jun 15 20:11:46 2018 +0000
@@ -0,0 +1,92 @@
+#include "PidController.hpp"
+
+#include "StaticDefs.hpp"
+
+PIDController::PIDController() {
+    _error = 0.0;
+    _integral = 0.0;
+    _loLimit = 0.0;
+    _hiLimit = 0.0;
+    _deadbandFlag = false;
+    _deadband = 0.0;
+    _headingFlag = false;   //default for pressure and depth
+}
+
+void PIDController::update(float position, float velocity, float dt) {
+    // error update equations
+    _error = _setPoint - position;
+    
+    _temp_velocity = velocity;
+    
+    // hack to get heading update correct
+    // need to make sure this makes physical sense
+    if (_headingFlag) {
+        if (_error >= 180){
+            _error = _error - 360.0;
+        }else if(_error <= -180){
+            _error = _error + 360.0;
+        }
+    }
+    
+    //calculate integral error term (adding anti-windup)
+    // ADDED k_aw or anti-windup term for bench testing (typically very small but anything from 0 to 0.9999...)
+    float AWgain = 0.1;    // AntiWindupGain
+    float integral_dot = _Pgain * _Igain * (_error - AWgain * abs(_error) * _integral);
+    
+    //calculate integral error term
+    //_integral = _integral + (_error*dt);  //original equation
+    _integral = _integral + integral_dot * dt;
+
+    // pid controller equation                                          //derivative error had the wrong sign
+    _output = _Pgain*_error + _Igain*_integral - _Dgain*velocity ;      //derivative_error = v_setpoint - v_actual * _Dgain
+
+    // limiting on output & integral anti-windup (preventing integral from getting extremely high)
+    // Get the current controller output, and stop integrating if it's saturated    
+    // one solution, stop integrating when you the position limits
+
+    // 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;
+}
+
+void PIDController::setHeadingFlag(bool heading_flag) {
+    _headingFlag = heading_flag;
+}
+
+void PIDController::setHiLimit(float high_limit) {
+    _hiLimit = high_limit;
+}
+
+void PIDController::setLoLimit(float low_limit){
+    _loLimit = low_limit;
+}
\ No newline at end of file