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@59:4c04d5c7aed1, 2018-06-15 (annotated)
- Committer:
- tnhnrl
- Date:
- Fri Jun 15 20:11:46 2018 +0000
- Revision:
- 59:4c04d5c7aed1
- Child:
- 92:52a91656458a
Menu only;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tnhnrl | 59:4c04d5c7aed1 | 1 | #include "PidController.hpp" |
tnhnrl | 59:4c04d5c7aed1 | 2 | |
tnhnrl | 59:4c04d5c7aed1 | 3 | #include "StaticDefs.hpp" |
tnhnrl | 59:4c04d5c7aed1 | 4 | |
tnhnrl | 59:4c04d5c7aed1 | 5 | PIDController::PIDController() { |
tnhnrl | 59:4c04d5c7aed1 | 6 | _error = 0.0; |
tnhnrl | 59:4c04d5c7aed1 | 7 | _integral = 0.0; |
tnhnrl | 59:4c04d5c7aed1 | 8 | _loLimit = 0.0; |
tnhnrl | 59:4c04d5c7aed1 | 9 | _hiLimit = 0.0; |
tnhnrl | 59:4c04d5c7aed1 | 10 | _deadbandFlag = false; |
tnhnrl | 59:4c04d5c7aed1 | 11 | _deadband = 0.0; |
tnhnrl | 59:4c04d5c7aed1 | 12 | _headingFlag = false; //default for pressure and depth |
tnhnrl | 59:4c04d5c7aed1 | 13 | } |
tnhnrl | 59:4c04d5c7aed1 | 14 | |
tnhnrl | 59:4c04d5c7aed1 | 15 | void PIDController::update(float position, float velocity, float dt) { |
tnhnrl | 59:4c04d5c7aed1 | 16 | // error update equations |
tnhnrl | 59:4c04d5c7aed1 | 17 | _error = _setPoint - position; |
tnhnrl | 59:4c04d5c7aed1 | 18 | |
tnhnrl | 59:4c04d5c7aed1 | 19 | _temp_velocity = velocity; |
tnhnrl | 59:4c04d5c7aed1 | 20 | |
tnhnrl | 59:4c04d5c7aed1 | 21 | // hack to get heading update correct |
tnhnrl | 59:4c04d5c7aed1 | 22 | // need to make sure this makes physical sense |
tnhnrl | 59:4c04d5c7aed1 | 23 | if (_headingFlag) { |
tnhnrl | 59:4c04d5c7aed1 | 24 | if (_error >= 180){ |
tnhnrl | 59:4c04d5c7aed1 | 25 | _error = _error - 360.0; |
tnhnrl | 59:4c04d5c7aed1 | 26 | }else if(_error <= -180){ |
tnhnrl | 59:4c04d5c7aed1 | 27 | _error = _error + 360.0; |
tnhnrl | 59:4c04d5c7aed1 | 28 | } |
tnhnrl | 59:4c04d5c7aed1 | 29 | } |
tnhnrl | 59:4c04d5c7aed1 | 30 | |
tnhnrl | 59:4c04d5c7aed1 | 31 | //calculate integral error term (adding anti-windup) |
tnhnrl | 59:4c04d5c7aed1 | 32 | // ADDED k_aw or anti-windup term for bench testing (typically very small but anything from 0 to 0.9999...) |
tnhnrl | 59:4c04d5c7aed1 | 33 | float AWgain = 0.1; // AntiWindupGain |
tnhnrl | 59:4c04d5c7aed1 | 34 | float integral_dot = _Pgain * _Igain * (_error - AWgain * abs(_error) * _integral); |
tnhnrl | 59:4c04d5c7aed1 | 35 | |
tnhnrl | 59:4c04d5c7aed1 | 36 | //calculate integral error term |
tnhnrl | 59:4c04d5c7aed1 | 37 | //_integral = _integral + (_error*dt); //original equation |
tnhnrl | 59:4c04d5c7aed1 | 38 | _integral = _integral + integral_dot * dt; |
tnhnrl | 59:4c04d5c7aed1 | 39 | |
tnhnrl | 59:4c04d5c7aed1 | 40 | // pid controller equation //derivative error had the wrong sign |
tnhnrl | 59:4c04d5c7aed1 | 41 | _output = _Pgain*_error + _Igain*_integral - _Dgain*velocity ; //derivative_error = v_setpoint - v_actual * _Dgain |
tnhnrl | 59:4c04d5c7aed1 | 42 | |
tnhnrl | 59:4c04d5c7aed1 | 43 | // limiting on output & integral anti-windup (preventing integral from getting extremely high) |
tnhnrl | 59:4c04d5c7aed1 | 44 | // Get the current controller output, and stop integrating if it's saturated |
tnhnrl | 59:4c04d5c7aed1 | 45 | // one solution, stop integrating when you the position limits |
tnhnrl | 59:4c04d5c7aed1 | 46 | |
tnhnrl | 59:4c04d5c7aed1 | 47 | // within deadband on error zeros output |
tnhnrl | 59:4c04d5c7aed1 | 48 | if (_deadbandFlag) { |
tnhnrl | 59:4c04d5c7aed1 | 49 | if (abs(_error) < _deadband) { |
tnhnrl | 59:4c04d5c7aed1 | 50 | _output = 0.0; |
tnhnrl | 59:4c04d5c7aed1 | 51 | } |
tnhnrl | 59:4c04d5c7aed1 | 52 | } |
tnhnrl | 59:4c04d5c7aed1 | 53 | } |
tnhnrl | 59:4c04d5c7aed1 | 54 | |
tnhnrl | 59:4c04d5c7aed1 | 55 | void PIDController::writeSetPoint(float cmd) { |
tnhnrl | 59:4c04d5c7aed1 | 56 | _setPoint = cmd; |
tnhnrl | 59:4c04d5c7aed1 | 57 | } |
tnhnrl | 59:4c04d5c7aed1 | 58 | |
tnhnrl | 59:4c04d5c7aed1 | 59 | float PIDController::getOutput() { |
tnhnrl | 59:4c04d5c7aed1 | 60 | return _output; |
tnhnrl | 59:4c04d5c7aed1 | 61 | } |
tnhnrl | 59:4c04d5c7aed1 | 62 | |
tnhnrl | 59:4c04d5c7aed1 | 63 | void PIDController::setPgain(float gain) { |
tnhnrl | 59:4c04d5c7aed1 | 64 | _Pgain = gain; |
tnhnrl | 59:4c04d5c7aed1 | 65 | } |
tnhnrl | 59:4c04d5c7aed1 | 66 | |
tnhnrl | 59:4c04d5c7aed1 | 67 | void PIDController::setIgain(float gain) { |
tnhnrl | 59:4c04d5c7aed1 | 68 | _Igain = gain; |
tnhnrl | 59:4c04d5c7aed1 | 69 | } |
tnhnrl | 59:4c04d5c7aed1 | 70 | |
tnhnrl | 59:4c04d5c7aed1 | 71 | void PIDController::setDgain(float gain) { |
tnhnrl | 59:4c04d5c7aed1 | 72 | _Dgain = gain; |
tnhnrl | 59:4c04d5c7aed1 | 73 | } |
tnhnrl | 59:4c04d5c7aed1 | 74 | |
tnhnrl | 59:4c04d5c7aed1 | 75 | void PIDController::toggleDeadBand(bool toggle) { |
tnhnrl | 59:4c04d5c7aed1 | 76 | _deadbandFlag = toggle; |
tnhnrl | 59:4c04d5c7aed1 | 77 | } |
tnhnrl | 59:4c04d5c7aed1 | 78 | void PIDController::setDeadBand(float deadband) { |
tnhnrl | 59:4c04d5c7aed1 | 79 | _deadband = deadband; |
tnhnrl | 59:4c04d5c7aed1 | 80 | } |
tnhnrl | 59:4c04d5c7aed1 | 81 | |
tnhnrl | 59:4c04d5c7aed1 | 82 | void PIDController::setHeadingFlag(bool heading_flag) { |
tnhnrl | 59:4c04d5c7aed1 | 83 | _headingFlag = heading_flag; |
tnhnrl | 59:4c04d5c7aed1 | 84 | } |
tnhnrl | 59:4c04d5c7aed1 | 85 | |
tnhnrl | 59:4c04d5c7aed1 | 86 | void PIDController::setHiLimit(float high_limit) { |
tnhnrl | 59:4c04d5c7aed1 | 87 | _hiLimit = high_limit; |
tnhnrl | 59:4c04d5c7aed1 | 88 | } |
tnhnrl | 59:4c04d5c7aed1 | 89 | |
tnhnrl | 59:4c04d5c7aed1 | 90 | void PIDController::setLoLimit(float low_limit){ |
tnhnrl | 59:4c04d5c7aed1 | 91 | _loLimit = low_limit; |
tnhnrl | 59:4c04d5c7aed1 | 92 | } |