FSG
/
Controller_
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: 7_20_17_FSG_ 7_21_17_FSG 7_26_17_FSG
Fork of
Controller
by 
Melissa Davis
controller.cpp@5:55085534a378, 2017-07-21 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 21 15:25:04 2017 +0000
- Revision:
- 5:55085534a378
- Parent:
- 4:503138a2f413
- Child:
- 6:29cf76bb9956
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| tzyoung | 0:2566234da01a | 1 | #include "controller.hpp" |
| tzyoung | 0:2566234da01a | 2 | #include "StaticDefs.hpp" |
| tzyoung | 0:2566234da01a | 3 | |
| tzyoung | 0:2566234da01a | 4 | PositionController::PositionController() |
| tzyoung | 0:2566234da01a | 5 | { |
| tzyoung | 0:2566234da01a | 6 | _integral = 0.0; |
| tzyoung | 0:2566234da01a | 7 | _lastTime = 0; |
| tzyoung | 0:2566234da01a | 8 | |
| tzyoung | 0:2566234da01a | 9 | } |
| tzyoung | 0:2566234da01a | 10 | |
| tzyoung | 0:2566234da01a | 11 | void PositionController::update(float position, float velocity, float dt) |
| tzyoung | 0:2566234da01a | 12 | { |
| tzyoung | 0:2566234da01a | 13 | |
| tzyoung | 0:2566234da01a | 14 | _error = _setPoint - position; |
| tzyoung | 0:2566234da01a | 15 | |
| tzyoung | 0:2566234da01a | 16 | _integral += (_error*dt); |
| tzyoung | 0:2566234da01a | 17 | |
| tzyoung | 0:2566234da01a | 18 | _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ; |
| tzyoung | 0:2566234da01a | 19 | |
| tzyoung | 0:2566234da01a | 20 | // limiting on output & integral anti-windup |
| tzyoung | 0:2566234da01a | 21 | if (_output > 1.0) { |
| tzyoung | 0:2566234da01a | 22 | _output = 1.0; |
| tzyoung | 0:2566234da01a | 23 | _integral -= _error*dt; //implement saturation instead of reset |
| tzyoung | 0:2566234da01a | 24 | } else if (_output < -1) { |
| tzyoung | 0:2566234da01a | 25 | _output = -1.0; |
| tzyoung | 0:2566234da01a | 26 | _integral += _error*dt; //implement saturation instead of reset |
| tzyoung | 0:2566234da01a | 27 | } |
| tzyoung | 0:2566234da01a | 28 | |
| tzyoung | 0:2566234da01a | 29 | //add in some deadband |
| tzyoung | 0:2566234da01a | 30 | if (_error < 0.5 && _error > -0.5) { |
| tzyoung | 0:2566234da01a | 31 | _output = 0.0; |
| tzyoung | 0:2566234da01a | 32 | } |
| tzyoung | 0:2566234da01a | 33 | } |
| tzyoung | 0:2566234da01a | 34 | |
| tzyoung | 0:2566234da01a | 35 | void PositionController::writeSetPoint(float cmd) |
| tzyoung | 0:2566234da01a | 36 | { |
| tzyoung | 0:2566234da01a | 37 | //33mm and 400mm are the safe allowable limits that the piston can go to |
| mdavis30 | 5:55085534a378 | 38 | _setPoint = clamp<float>(cmd, -10.0, 380.0); //changed 400 to 500 on 6/5/17 //33 to 50 |
| tzyoung | 0:2566234da01a | 39 | } |
| tzyoung | 0:2566234da01a | 40 | |
| tzyoung | 0:2566234da01a | 41 | float PositionController::getOutput() |
| tzyoung | 0:2566234da01a | 42 | { |
| tzyoung | 0:2566234da01a | 43 | return _output; |
| tzyoung | 0:2566234da01a | 44 | } |
| tzyoung | 0:2566234da01a | 45 | |
| tzyoung | 0:2566234da01a | 46 | /*int DepthController::readConfiguration() |
| tzyoung | 0:2566234da01a | 47 | { |
| tzyoung | 0:2566234da01a | 48 | ConfigFile cfg; |
| tzyoung | 0:2566234da01a | 49 | int count = 0; |
| tzyoung | 0:2566234da01a | 50 | cfg.read("/local/gains.txt"); |
| tzyoung | 0:2566234da01a | 51 | if (cfg.getValue("PDepth", &Pgain , 0.0)) { |
| tzyoung | 0:2566234da01a | 52 | count++; |
| tzyoung | 0:2566234da01a | 53 | } |
| tzyoung | 0:2566234da01a | 54 | if (cfg.getValue("IDepth", &Igain , 0.0)) { |
| tzyoung | 0:2566234da01a | 55 | count++; |
| tzyoung | 0:2566234da01a | 56 | } |
| tzyoung | 0:2566234da01a | 57 | if (cfg.getValue("DDepth", &Dgain , 0.0)) { |
| tzyoung | 0:2566234da01a | 58 | count++; |
| tzyoung | 0:2566234da01a | 59 | } |
| tzyoung | 0:2566234da01a | 60 | //uncomment the following to debug config file issues |
| tzyoung | 0:2566234da01a | 61 | //pc().printf("Pgain:%2.2f Igain:%2.2f Dgain:%2.2f", Pgain, Igain, Dgain); |
| tzyoung | 0:2566234da01a | 62 | |
| tzyoung | 0:2566234da01a | 63 | return count; |
| tzyoung | 0:2566234da01a | 64 | } */ |
| tzyoung | 0:2566234da01a | 65 | |
| tzyoung | 0:2566234da01a | 66 | //void DepthController::setConfigFlag() |
| tzyoung | 0:2566234da01a | 67 | //{ |
| tzyoung | 0:2566234da01a | 68 | // configFlag = true; |
| tzyoung | 0:2566234da01a | 69 | //} |
| tzyoung | 0:2566234da01a | 70 | |
| tzyoung | 0:2566234da01a | 71 | void PositionController::setPgain(float gain) |
| tzyoung | 0:2566234da01a | 72 | { |
| tzyoung | 0:2566234da01a | 73 | _Pgain = gain; |
| tzyoung | 0:2566234da01a | 74 | pc().printf("\n\rPgain Depth Controller = %f\n\r", _Pgain); |
| tzyoung | 0:2566234da01a | 75 | } |
| tzyoung | 0:2566234da01a | 76 | |
| tzyoung | 0:2566234da01a | 77 | void PositionController::setIgain(float gain) |
| tzyoung | 0:2566234da01a | 78 | { |
| tzyoung | 0:2566234da01a | 79 | _Igain = gain; |
| tzyoung | 0:2566234da01a | 80 | pc().printf("\n\rIgain Depth Controller = %f\n\r", _Igain); |
| tzyoung | 0:2566234da01a | 81 | } |
| tzyoung | 0:2566234da01a | 82 | |
| tzyoung | 0:2566234da01a | 83 | void PositionController::setDgain(float gain) |
| tzyoung | 0:2566234da01a | 84 | { |
| tzyoung | 0:2566234da01a | 85 | _Dgain = gain; |
| tzyoung | 0:2566234da01a | 86 | pc().printf("\n\rDgain Depth Controller = %f\n\r", _Dgain); |
| mdavis30 | 1:1cc8a8347f22 | 87 | } |