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