update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
PidController/PidController.cpp
- Committer:
- joel_ssc
- Date:
- 2019-09-13
- Revision:
- 104:426224a55f5f
- Parent:
- 102:0f430de62447
File content as of revision 104:426224a55f5f:
#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){ //technically, this adjustment should continue until error is less than 180. _error = _error - 360.0; }else if(_error <= -180){ // likewise here, continue until error greater than -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; }