update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
OuterLoop/OuterLoop.cpp@11:3b241ecb75ed, 2017-10-27 (annotated)
- Committer:
- danstrider
- Date:
- Fri Oct 27 00:37:32 2017 +0000
- Revision:
- 11:3b241ecb75ed
- Parent:
- 10:085ab7328054
- Child:
- 14:85b64a4d08e8
This version has been in the pool, working with all the hardware. Had occasional string pot problems and got stuck in RISE, letting the battery stall out against the endcap.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
danstrider | 10:085ab7328054 | 1 | #include "mbed.h" |
danstrider | 10:085ab7328054 | 2 | #include "OuterLoop.hpp" |
danstrider | 10:085ab7328054 | 3 | #include "StaticDefs.hpp" |
danstrider | 10:085ab7328054 | 4 | |
danstrider | 10:085ab7328054 | 5 | // this is where the variables that can be set are set when the object is created |
danstrider | 10:085ab7328054 | 6 | OuterLoop::OuterLoop(float interval, int sensor): |
danstrider | 10:085ab7328054 | 7 | _filter(), |
danstrider | 10:085ab7328054 | 8 | _pid(), |
danstrider | 10:085ab7328054 | 9 | _pulse() |
danstrider | 10:085ab7328054 | 10 | { |
danstrider | 10:085ab7328054 | 11 | _Pgain = 0.5; |
danstrider | 10:085ab7328054 | 12 | _Igain = 0.0; |
danstrider | 10:085ab7328054 | 13 | _Dgain = 0.1; |
danstrider | 10:085ab7328054 | 14 | |
danstrider | 10:085ab7328054 | 15 | _filterFrequency = 2.0; |
danstrider | 10:085ab7328054 | 16 | _deadband = 0.0; |
danstrider | 10:085ab7328054 | 17 | |
danstrider | 10:085ab7328054 | 18 | _dt = interval; |
danstrider | 10:085ab7328054 | 19 | _sensor = sensor; // select the sensor ... hacky |
danstrider | 10:085ab7328054 | 20 | } |
danstrider | 10:085ab7328054 | 21 | |
danstrider | 10:085ab7328054 | 22 | // initialize and start all of the member objects. |
danstrider | 10:085ab7328054 | 23 | void OuterLoop::init() { |
danstrider | 10:085ab7328054 | 24 | // load gains into pid controller this should eventually be in a config file |
danstrider | 10:085ab7328054 | 25 | setFilterFrequency(_filterFrequency); |
danstrider | 10:085ab7328054 | 26 | setControllerP(_Pgain); |
danstrider | 10:085ab7328054 | 27 | setControllerI(_Igain); |
danstrider | 10:085ab7328054 | 28 | setControllerD(_Dgain); |
danstrider | 10:085ab7328054 | 29 | |
danstrider | 10:085ab7328054 | 30 | // setup the controller object |
danstrider | 10:085ab7328054 | 31 | toggleDeadband(true); |
danstrider | 10:085ab7328054 | 32 | setDeadband(_deadband); |
danstrider | 10:085ab7328054 | 33 | } |
danstrider | 10:085ab7328054 | 34 | |
danstrider | 10:085ab7328054 | 35 | // attaches the update function to an interval timer |
danstrider | 10:085ab7328054 | 36 | void OuterLoop::start() { |
danstrider | 10:085ab7328054 | 37 | _pulse.attach(this,&OuterLoop::update, _dt); |
danstrider | 10:085ab7328054 | 38 | } |
danstrider | 10:085ab7328054 | 39 | |
danstrider | 10:085ab7328054 | 40 | // detaches the update function from the interval timer |
danstrider | 10:085ab7328054 | 41 | void OuterLoop::stop() { |
danstrider | 10:085ab7328054 | 42 | _pulse.detach(); |
danstrider | 10:085ab7328054 | 43 | } |
danstrider | 10:085ab7328054 | 44 | |
danstrider | 10:085ab7328054 | 45 | // todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this |
danstrider | 10:085ab7328054 | 46 | void OuterLoop::update() { |
danstrider | 10:085ab7328054 | 47 | // update the position velocity filter |
danstrider | 10:085ab7328054 | 48 | if (_sensor == 0) { |
danstrider | 10:085ab7328054 | 49 | _sensorVal = depth().getDepth(); |
danstrider | 10:085ab7328054 | 50 | } else if (_sensor == 1) { |
danstrider | 10:085ab7328054 | 51 | _sensorVal = imu().getPitch(); |
danstrider | 10:085ab7328054 | 52 | } else if (_sensor == 2) { |
danstrider | 10:085ab7328054 | 53 | _sensorVal = imu().getHeading(); |
danstrider | 10:085ab7328054 | 54 | } else { |
danstrider | 10:085ab7328054 | 55 | error("\n\r This sensor option does not exist"); |
danstrider | 10:085ab7328054 | 56 | } |
danstrider | 10:085ab7328054 | 57 | |
danstrider | 10:085ab7328054 | 58 | // use the sensor reading to update the PVF |
danstrider | 10:085ab7328054 | 59 | _filter.update(_dt, _sensorVal); |
danstrider | 10:085ab7328054 | 60 | |
danstrider | 10:085ab7328054 | 61 | // refresh the filter results and load into class variables |
danstrider | 10:085ab7328054 | 62 | refreshPVState(); |
danstrider | 10:085ab7328054 | 63 | |
danstrider | 10:085ab7328054 | 64 | // update the PID controller with latest data |
danstrider | 10:085ab7328054 | 65 | _pid.update(_position, _velocity, _filter.getDt()); |
danstrider | 10:085ab7328054 | 66 | } |
danstrider | 10:085ab7328054 | 67 | |
danstrider | 10:085ab7328054 | 68 | void OuterLoop::setCommand(float value) { |
danstrider | 10:085ab7328054 | 69 | _SetPoint = value; |
danstrider | 10:085ab7328054 | 70 | _pid.writeSetPoint(_SetPoint); |
danstrider | 10:085ab7328054 | 71 | } |
danstrider | 10:085ab7328054 | 72 | |
danstrider | 11:3b241ecb75ed | 73 | float OuterLoop::getCommand() { |
danstrider | 11:3b241ecb75ed | 74 | return _SetPoint; |
danstrider | 11:3b241ecb75ed | 75 | } |
danstrider | 11:3b241ecb75ed | 76 | |
danstrider | 10:085ab7328054 | 77 | float OuterLoop::getOutput() { |
danstrider | 11:3b241ecb75ed | 78 | /* PID output + offset to drive the motors to the correct position */ |
danstrider | 11:3b241ecb75ed | 79 | return _pid.getOutput() + _offset; |
danstrider | 10:085ab7328054 | 80 | } |
danstrider | 10:085ab7328054 | 81 | |
danstrider | 10:085ab7328054 | 82 | void OuterLoop::refreshPVState() { |
danstrider | 10:085ab7328054 | 83 | _position = _filter.getPosition(); |
danstrider | 10:085ab7328054 | 84 | _velocity = _filter.getVelocity(); |
danstrider | 10:085ab7328054 | 85 | } |
danstrider | 10:085ab7328054 | 86 | |
danstrider | 10:085ab7328054 | 87 | float OuterLoop::getPosition() { |
danstrider | 10:085ab7328054 | 88 | return _position; |
danstrider | 10:085ab7328054 | 89 | } |
danstrider | 10:085ab7328054 | 90 | |
danstrider | 10:085ab7328054 | 91 | float OuterLoop::getVelocity() { |
danstrider | 10:085ab7328054 | 92 | return _velocity; |
danstrider | 10:085ab7328054 | 93 | } |
danstrider | 10:085ab7328054 | 94 | |
danstrider | 10:085ab7328054 | 95 | void OuterLoop::setControllerP(float P) { |
danstrider | 10:085ab7328054 | 96 | _Pgain = P; |
danstrider | 10:085ab7328054 | 97 | _pid.setPgain(_Pgain); |
danstrider | 10:085ab7328054 | 98 | } |
danstrider | 10:085ab7328054 | 99 | |
danstrider | 10:085ab7328054 | 100 | float OuterLoop::getControllerP() { |
danstrider | 10:085ab7328054 | 101 | return _Pgain; |
danstrider | 10:085ab7328054 | 102 | } |
danstrider | 10:085ab7328054 | 103 | |
danstrider | 10:085ab7328054 | 104 | void OuterLoop::setControllerI(float I) { |
danstrider | 10:085ab7328054 | 105 | _Igain = I; |
danstrider | 10:085ab7328054 | 106 | _pid.setIgain(_Igain); |
danstrider | 10:085ab7328054 | 107 | } |
danstrider | 10:085ab7328054 | 108 | |
danstrider | 10:085ab7328054 | 109 | float OuterLoop::getControllerI() { |
danstrider | 10:085ab7328054 | 110 | return _Igain; |
danstrider | 10:085ab7328054 | 111 | } |
danstrider | 10:085ab7328054 | 112 | |
danstrider | 10:085ab7328054 | 113 | void OuterLoop::setControllerD(float D) { |
danstrider | 10:085ab7328054 | 114 | _Dgain = D; |
danstrider | 10:085ab7328054 | 115 | _pid.setDgain(_Dgain); |
danstrider | 10:085ab7328054 | 116 | } |
danstrider | 10:085ab7328054 | 117 | |
danstrider | 10:085ab7328054 | 118 | float OuterLoop::getControllerD() { |
danstrider | 10:085ab7328054 | 119 | return _Dgain; |
danstrider | 10:085ab7328054 | 120 | } |
danstrider | 10:085ab7328054 | 121 | |
danstrider | 10:085ab7328054 | 122 | void OuterLoop::setFilterFrequency(float frequency) { |
danstrider | 10:085ab7328054 | 123 | _filterFrequency = frequency; |
danstrider | 10:085ab7328054 | 124 | _filter.writeWn(frequency); |
danstrider | 10:085ab7328054 | 125 | } |
danstrider | 10:085ab7328054 | 126 | |
danstrider | 10:085ab7328054 | 127 | void OuterLoop::setDeadband(float deadband) { |
danstrider | 10:085ab7328054 | 128 | _deadband = deadband; |
danstrider | 10:085ab7328054 | 129 | _pid.setDeadBand(_deadband); |
danstrider | 10:085ab7328054 | 130 | } |
danstrider | 10:085ab7328054 | 131 | |
danstrider | 10:085ab7328054 | 132 | bool OuterLoop::toggleDeadband(bool toggle) { |
danstrider | 10:085ab7328054 | 133 | _pid.toggleDeadBand(toggle); |
danstrider | 10:085ab7328054 | 134 | return toggle; |
danstrider | 11:3b241ecb75ed | 135 | } |
danstrider | 11:3b241ecb75ed | 136 | |
danstrider | 11:3b241ecb75ed | 137 | void OuterLoop::setOutputOffset(float offset) { |
danstrider | 11:3b241ecb75ed | 138 | _offset = offset; |
danstrider | 11:3b241ecb75ed | 139 | } |
danstrider | 11:3b241ecb75ed | 140 | |
danstrider | 11:3b241ecb75ed | 141 | float OuterLoop::getOutputOffset() { |
danstrider | 11:3b241ecb75ed | 142 | return _offset; |
danstrider | 11:3b241ecb75ed | 143 | } |