update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
PidController.cpp
00001 #include "PidController.hpp" 00002 00003 #include "StaticDefs.hpp" 00004 00005 PIDController::PIDController() { 00006 _error = 0.0; 00007 _integral = 0.0; 00008 _loLimit = 0.0; 00009 _hiLimit = 0.0; 00010 _deadbandFlag = false; 00011 _deadband = 0.0; 00012 _headingFlag = false; //default for pressure and depth 00013 } 00014 00015 void PIDController::update(float position, float velocity, float dt) { 00016 // error update equations 00017 _error = _setPoint - position; 00018 00019 _temp_velocity = velocity; 00020 00021 // hack to get heading update correct 00022 // need to make sure this makes physical sense 00023 if (_headingFlag) { 00024 if (_error >= 180){ //technically, this adjustment should continue until error is less than 180. 00025 _error = _error - 360.0; 00026 }else if(_error <= -180){ // likewise here, continue until error greater than -180 00027 _error = _error + 360.0; 00028 } 00029 } 00030 00031 //calculate integral error term (adding anti-windup) 00032 // ADDED k_aw or anti-windup term for bench testing (typically very small but anything from 0 to 0.9999...) 00033 float AWgain = 0.1; // AntiWindupGain 00034 float integral_dot = _Pgain * _Igain * (_error - AWgain * abs(_error) * _integral); 00035 00036 //calculate integral error term 00037 //_integral = _integral + (_error*dt); //original equation 00038 _integral = _integral + integral_dot * dt; 00039 00040 // pid controller equation //derivative error had the wrong sign 00041 _output = _Pgain*_error + _Igain*_integral - _Dgain*velocity ; //derivative_error = v_setpoint - v_actual * _Dgain 00042 00043 // limiting on output & integral anti-windup (preventing integral from getting extremely high) 00044 // Get the current controller output, and stop integrating if it's saturated 00045 // one solution, stop integrating when you the position limits 00046 00047 // within deadband on error zeros output 00048 if (_deadbandFlag) { 00049 if (abs(_error) < _deadband) { 00050 _output = 0.0; 00051 } 00052 } 00053 } 00054 00055 void PIDController::writeSetPoint(float cmd) { 00056 _setPoint = cmd; 00057 } 00058 00059 float PIDController::getOutput() { 00060 return _output; 00061 } 00062 00063 void PIDController::setPgain(float gain) { 00064 _Pgain = gain; 00065 } 00066 00067 void PIDController::setIgain(float gain) { 00068 _Igain = gain; 00069 } 00070 00071 void PIDController::setDgain(float gain) { 00072 _Dgain = gain; 00073 } 00074 00075 void PIDController::toggleDeadBand(bool toggle) { 00076 _deadbandFlag = toggle; 00077 } 00078 void PIDController::setDeadBand(float deadband) { 00079 _deadband = deadband; 00080 } 00081 00082 void PIDController::setHeadingFlag(bool heading_flag) { 00083 _headingFlag = heading_flag; 00084 } 00085 00086 void PIDController::setHiLimit(float high_limit) { 00087 _hiLimit = high_limit; 00088 } 00089 00090 void PIDController::setLoLimit(float low_limit){ 00091 _loLimit = low_limit; 00092 }
Generated on Wed Jul 13 2022 15:28:17 by 1.7.2