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
PidController/PidController.cpp
- Committer:
- tnhnrl
- Date:
- 2018-06-15
- Revision:
- 59:4c04d5c7aed1
- Child:
- 92:52a91656458a
File content as of revision 59:4c04d5c7aed1:
#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; }