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){  //technincally, 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 }