update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
OuterLoop/OuterLoop.cpp@10:085ab7328054, 2017-10-23 (annotated)
- Committer:
- danstrider
- Date:
- Mon Oct 23 12:50:53 2017 +0000
- Revision:
- 10:085ab7328054
- Child:
- 11:3b241ecb75ed
checked out on the hardware
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 | 10:085ab7328054 | 73 | float OuterLoop::getOutput() { |
danstrider | 10:085ab7328054 | 74 | return _pid.getOutput(); |
danstrider | 10:085ab7328054 | 75 | } |
danstrider | 10:085ab7328054 | 76 | |
danstrider | 10:085ab7328054 | 77 | void OuterLoop::refreshPVState() { |
danstrider | 10:085ab7328054 | 78 | _position = _filter.getPosition(); |
danstrider | 10:085ab7328054 | 79 | _velocity = _filter.getVelocity(); |
danstrider | 10:085ab7328054 | 80 | } |
danstrider | 10:085ab7328054 | 81 | |
danstrider | 10:085ab7328054 | 82 | float OuterLoop::getPosition() { |
danstrider | 10:085ab7328054 | 83 | return _position; |
danstrider | 10:085ab7328054 | 84 | } |
danstrider | 10:085ab7328054 | 85 | |
danstrider | 10:085ab7328054 | 86 | float OuterLoop::getVelocity() { |
danstrider | 10:085ab7328054 | 87 | return _velocity; |
danstrider | 10:085ab7328054 | 88 | } |
danstrider | 10:085ab7328054 | 89 | |
danstrider | 10:085ab7328054 | 90 | void OuterLoop::setControllerP(float P) { |
danstrider | 10:085ab7328054 | 91 | _Pgain = P; |
danstrider | 10:085ab7328054 | 92 | _pid.setPgain(_Pgain); |
danstrider | 10:085ab7328054 | 93 | } |
danstrider | 10:085ab7328054 | 94 | |
danstrider | 10:085ab7328054 | 95 | float OuterLoop::getControllerP() { |
danstrider | 10:085ab7328054 | 96 | return _Pgain; |
danstrider | 10:085ab7328054 | 97 | } |
danstrider | 10:085ab7328054 | 98 | |
danstrider | 10:085ab7328054 | 99 | void OuterLoop::setControllerI(float I) { |
danstrider | 10:085ab7328054 | 100 | _Igain = I; |
danstrider | 10:085ab7328054 | 101 | _pid.setIgain(_Igain); |
danstrider | 10:085ab7328054 | 102 | } |
danstrider | 10:085ab7328054 | 103 | |
danstrider | 10:085ab7328054 | 104 | float OuterLoop::getControllerI() { |
danstrider | 10:085ab7328054 | 105 | return _Igain; |
danstrider | 10:085ab7328054 | 106 | } |
danstrider | 10:085ab7328054 | 107 | |
danstrider | 10:085ab7328054 | 108 | void OuterLoop::setControllerD(float D) { |
danstrider | 10:085ab7328054 | 109 | _Dgain = D; |
danstrider | 10:085ab7328054 | 110 | _pid.setDgain(_Dgain); |
danstrider | 10:085ab7328054 | 111 | } |
danstrider | 10:085ab7328054 | 112 | |
danstrider | 10:085ab7328054 | 113 | float OuterLoop::getControllerD() { |
danstrider | 10:085ab7328054 | 114 | return _Dgain; |
danstrider | 10:085ab7328054 | 115 | } |
danstrider | 10:085ab7328054 | 116 | |
danstrider | 10:085ab7328054 | 117 | void OuterLoop::setFilterFrequency(float frequency) { |
danstrider | 10:085ab7328054 | 118 | _filterFrequency = frequency; |
danstrider | 10:085ab7328054 | 119 | _filter.writeWn(frequency); |
danstrider | 10:085ab7328054 | 120 | } |
danstrider | 10:085ab7328054 | 121 | |
danstrider | 10:085ab7328054 | 122 | void OuterLoop::setDeadband(float deadband) { |
danstrider | 10:085ab7328054 | 123 | _deadband = deadband; |
danstrider | 10:085ab7328054 | 124 | _pid.setDeadBand(_deadband); |
danstrider | 10:085ab7328054 | 125 | } |
danstrider | 10:085ab7328054 | 126 | |
danstrider | 10:085ab7328054 | 127 | bool OuterLoop::toggleDeadband(bool toggle) { |
danstrider | 10:085ab7328054 | 128 | _pid.toggleDeadBand(toggle); |
danstrider | 10:085ab7328054 | 129 | return toggle; |
danstrider | 10:085ab7328054 | 130 | } |