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@6:29cf76bb9956, 2017-07-28 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 28 18:49:24 2017 +0000
- Revision:
- 6:29cf76bb9956
- Parent:
- 5:55085534a378
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 | 6:29cf76bb9956 | 38 | _setPoint = clamp<float>(cmd, 10.0, 380.0); //changed 400 to 500 on 6/5/17 //33 to 50 |
| mdavis30 | 6:29cf76bb9956 | 39 | } |
| mdavis30 | 6:29cf76bb9956 | 40 | |
| mdavis30 | 6:29cf76bb9956 | 41 | |
| mdavis30 | 6:29cf76bb9956 | 42 | ///////////////////Add the new bounds here!!!!!!!!!for the linear actuator!!!!//////////////////// |
| mdavis30 | 6:29cf76bb9956 | 43 | void PositionController::writeSetPoint_la(float cmd) |
| mdavis30 | 6:29cf76bb9956 | 44 | { |
| mdavis30 | 6:29cf76bb9956 | 45 | //33mm and 400mm are the safe allowable limits that the piston can go to |
| mdavis30 | 6:29cf76bb9956 | 46 | _setPoint = clamp<float>(cmd, 1.0, 100.0); //changed 400 to 500 on 6/5/17 //33 to 50 |
| tzyoung | 0:2566234da01a | 47 | } |
| tzyoung | 0:2566234da01a | 48 | |
| tzyoung | 0:2566234da01a | 49 | float PositionController::getOutput() |
| tzyoung | 0:2566234da01a | 50 | { |
| tzyoung | 0:2566234da01a | 51 | return _output; |
| tzyoung | 0:2566234da01a | 52 | } |
| tzyoung | 0:2566234da01a | 53 | |
| tzyoung | 0:2566234da01a | 54 | /*int DepthController::readConfiguration() |
| tzyoung | 0:2566234da01a | 55 | { |
| tzyoung | 0:2566234da01a | 56 | ConfigFile cfg; |
| tzyoung | 0:2566234da01a | 57 | int count = 0; |
| tzyoung | 0:2566234da01a | 58 | cfg.read("/local/gains.txt"); |
| tzyoung | 0:2566234da01a | 59 | if (cfg.getValue("PDepth", &Pgain , 0.0)) { |
| tzyoung | 0:2566234da01a | 60 | count++; |
| tzyoung | 0:2566234da01a | 61 | } |
| tzyoung | 0:2566234da01a | 62 | if (cfg.getValue("IDepth", &Igain , 0.0)) { |
| tzyoung | 0:2566234da01a | 63 | count++; |
| tzyoung | 0:2566234da01a | 64 | } |
| tzyoung | 0:2566234da01a | 65 | if (cfg.getValue("DDepth", &Dgain , 0.0)) { |
| tzyoung | 0:2566234da01a | 66 | count++; |
| tzyoung | 0:2566234da01a | 67 | } |
| tzyoung | 0:2566234da01a | 68 | //uncomment the following to debug config file issues |
| tzyoung | 0:2566234da01a | 69 | //pc().printf("Pgain:%2.2f Igain:%2.2f Dgain:%2.2f", Pgain, Igain, Dgain); |
| tzyoung | 0:2566234da01a | 70 | |
| tzyoung | 0:2566234da01a | 71 | return count; |
| tzyoung | 0:2566234da01a | 72 | } */ |
| tzyoung | 0:2566234da01a | 73 | |
| tzyoung | 0:2566234da01a | 74 | //void DepthController::setConfigFlag() |
| tzyoung | 0:2566234da01a | 75 | //{ |
| tzyoung | 0:2566234da01a | 76 | // configFlag = true; |
| tzyoung | 0:2566234da01a | 77 | //} |
| tzyoung | 0:2566234da01a | 78 | |
| tzyoung | 0:2566234da01a | 79 | void PositionController::setPgain(float gain) |
| tzyoung | 0:2566234da01a | 80 | { |
| tzyoung | 0:2566234da01a | 81 | _Pgain = gain; |
| tzyoung | 0:2566234da01a | 82 | pc().printf("\n\rPgain Depth Controller = %f\n\r", _Pgain); |
| tzyoung | 0:2566234da01a | 83 | } |
| tzyoung | 0:2566234da01a | 84 | |
| tzyoung | 0:2566234da01a | 85 | void PositionController::setIgain(float gain) |
| tzyoung | 0:2566234da01a | 86 | { |
| tzyoung | 0:2566234da01a | 87 | _Igain = gain; |
| tzyoung | 0:2566234da01a | 88 | pc().printf("\n\rIgain Depth Controller = %f\n\r", _Igain); |
| tzyoung | 0:2566234da01a | 89 | } |
| tzyoung | 0:2566234da01a | 90 | |
| tzyoung | 0:2566234da01a | 91 | void PositionController::setDgain(float gain) |
| tzyoung | 0:2566234da01a | 92 | { |
| tzyoung | 0:2566234da01a | 93 | _Dgain = gain; |
| tzyoung | 0:2566234da01a | 94 | pc().printf("\n\rDgain Depth Controller = %f\n\r", _Dgain); |
| mdavis30 | 1:1cc8a8347f22 | 95 | } |