update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
PidController/PidController.cpp@10:085ab7328054, 2017-10-23 (annotated)
- Committer:
- danstrider
- Date:
- Mon Oct 23 12:50:53 2017 +0000
- Revision:
- 10:085ab7328054
checked out on the hardware
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
danstrider | 10:085ab7328054 | 1 | #include "PidController.hpp" |
danstrider | 10:085ab7328054 | 2 | |
danstrider | 10:085ab7328054 | 3 | PIDController::PIDController() { |
danstrider | 10:085ab7328054 | 4 | _error = 0.0; |
danstrider | 10:085ab7328054 | 5 | _integral = 0.0; |
danstrider | 10:085ab7328054 | 6 | _loLimit = 0.0; |
danstrider | 10:085ab7328054 | 7 | _hiLimit = 0.0; |
danstrider | 10:085ab7328054 | 8 | _deadbandFlag = false; |
danstrider | 10:085ab7328054 | 9 | _deadband = 0.0; |
danstrider | 10:085ab7328054 | 10 | } |
danstrider | 10:085ab7328054 | 11 | |
danstrider | 10:085ab7328054 | 12 | void PIDController::update(float position, float velocity, float dt) { |
danstrider | 10:085ab7328054 | 13 | // error update equations |
danstrider | 10:085ab7328054 | 14 | _error = _setPoint - position; |
danstrider | 10:085ab7328054 | 15 | _integral += (_error*dt); |
danstrider | 10:085ab7328054 | 16 | |
danstrider | 10:085ab7328054 | 17 | // pid controller equation |
danstrider | 10:085ab7328054 | 18 | _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ; |
danstrider | 10:085ab7328054 | 19 | |
danstrider | 10:085ab7328054 | 20 | // limiting on output & integral anti-windup |
danstrider | 10:085ab7328054 | 21 | if (_hiLimit != 0.0 and _output > _hiLimit) { |
danstrider | 10:085ab7328054 | 22 | _output = _hiLimit; |
danstrider | 10:085ab7328054 | 23 | _integral -= _error*dt; // implement saturation |
danstrider | 10:085ab7328054 | 24 | } |
danstrider | 10:085ab7328054 | 25 | if (_loLimit != 0.0 and _output < _loLimit) { |
danstrider | 10:085ab7328054 | 26 | _output = _loLimit; |
danstrider | 10:085ab7328054 | 27 | _integral += _error*dt; // implement saturation |
danstrider | 10:085ab7328054 | 28 | } |
danstrider | 10:085ab7328054 | 29 | |
danstrider | 10:085ab7328054 | 30 | // within deadband on error zeros output |
danstrider | 10:085ab7328054 | 31 | if (_deadbandFlag) { |
danstrider | 10:085ab7328054 | 32 | if (abs(_error) < _deadband) { |
danstrider | 10:085ab7328054 | 33 | _output = 0.0; |
danstrider | 10:085ab7328054 | 34 | } |
danstrider | 10:085ab7328054 | 35 | } |
danstrider | 10:085ab7328054 | 36 | } |
danstrider | 10:085ab7328054 | 37 | |
danstrider | 10:085ab7328054 | 38 | void PIDController::writeSetPoint(float cmd) { |
danstrider | 10:085ab7328054 | 39 | _setPoint = cmd; |
danstrider | 10:085ab7328054 | 40 | } |
danstrider | 10:085ab7328054 | 41 | |
danstrider | 10:085ab7328054 | 42 | float PIDController::getOutput() { |
danstrider | 10:085ab7328054 | 43 | return _output; |
danstrider | 10:085ab7328054 | 44 | } |
danstrider | 10:085ab7328054 | 45 | |
danstrider | 10:085ab7328054 | 46 | void PIDController::setPgain(float gain) { |
danstrider | 10:085ab7328054 | 47 | _Pgain = gain; |
danstrider | 10:085ab7328054 | 48 | } |
danstrider | 10:085ab7328054 | 49 | |
danstrider | 10:085ab7328054 | 50 | void PIDController::setIgain(float gain) { |
danstrider | 10:085ab7328054 | 51 | _Igain = gain; |
danstrider | 10:085ab7328054 | 52 | } |
danstrider | 10:085ab7328054 | 53 | |
danstrider | 10:085ab7328054 | 54 | void PIDController::setDgain(float gain) { |
danstrider | 10:085ab7328054 | 55 | _Dgain = gain; |
danstrider | 10:085ab7328054 | 56 | } |
danstrider | 10:085ab7328054 | 57 | |
danstrider | 10:085ab7328054 | 58 | void PIDController::toggleDeadBand(bool toggle) { |
danstrider | 10:085ab7328054 | 59 | _deadbandFlag = toggle; |
danstrider | 10:085ab7328054 | 60 | } |
danstrider | 10:085ab7328054 | 61 | void PIDController::setDeadBand(float deadband) { |
danstrider | 10:085ab7328054 | 62 | _deadband = deadband; |
danstrider | 10:085ab7328054 | 63 | } |