most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
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