update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
OuterLoop/OuterLoop.cpp@99:9d0849f5fcd7, 2019-06-28 (annotated)
- Committer:
- CodyMarquardt
- Date:
- Fri Jun 28 13:59:11 2019 +0000
- Revision:
- 99:9d0849f5fcd7
- Parent:
- 87:6d95f853dab3
Program has bugs. Committing in order to access in MBED studio to debug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tnhnrl | 62:d502889e74f1 | 1 | #include "mbed.h" |
tnhnrl | 62:d502889e74f1 | 2 | #include "OuterLoop.hpp" |
tnhnrl | 62:d502889e74f1 | 3 | #include "StaticDefs.hpp" |
tnhnrl | 62:d502889e74f1 | 4 | |
tnhnrl | 62:d502889e74f1 | 5 | // this is where the variables that can be set are set when the object is created |
tnhnrl | 62:d502889e74f1 | 6 | OuterLoop::OuterLoop(float interval, int sensor): |
tnhnrl | 62:d502889e74f1 | 7 | _filter(), |
tnhnrl | 62:d502889e74f1 | 8 | _pid(), |
tnhnrl | 62:d502889e74f1 | 9 | _pulse() |
tnhnrl | 62:d502889e74f1 | 10 | { |
tnhnrl | 62:d502889e74f1 | 11 | _Pgain = 0.5; |
tnhnrl | 62:d502889e74f1 | 12 | _Igain = 0.0; |
tnhnrl | 62:d502889e74f1 | 13 | _Dgain = 0.1; |
tnhnrl | 62:d502889e74f1 | 14 | |
tnhnrl | 62:d502889e74f1 | 15 | _filterFrequency = 2.0; |
tnhnrl | 62:d502889e74f1 | 16 | _deadband = 0.0; |
tnhnrl | 62:d502889e74f1 | 17 | |
tnhnrl | 62:d502889e74f1 | 18 | _dt = interval; |
tnhnrl | 62:d502889e74f1 | 19 | _sensor = sensor; // select the sensor ... hacky |
tnhnrl | 62:d502889e74f1 | 20 | setIHiLimit(3.0); //give each outerloop instance an integral saturation limit of some default |
tnhnrl | 62:d502889e74f1 | 21 | setILoLimit(3.0); |
tnhnrl | 62:d502889e74f1 | 22 | |
tnhnrl | 62:d502889e74f1 | 23 | if (_sensor == 2) { |
tnhnrl | 74:d281aaef9766 | 24 | _pid.setHeadingFlag(true); //used to update the handling for the heading PID control loop |
joel_ssc | 87:6d95f853dab3 | 25 | _filter.setHeadingFlag(true); |
tnhnrl | 62:d502889e74f1 | 26 | } |
tnhnrl | 62:d502889e74f1 | 27 | } |
tnhnrl | 62:d502889e74f1 | 28 | |
tnhnrl | 62:d502889e74f1 | 29 | // initialize and start all of the member objects. |
tnhnrl | 62:d502889e74f1 | 30 | void OuterLoop::init() { |
tnhnrl | 62:d502889e74f1 | 31 | // load gains into pid controller this should eventually be in a config file |
tnhnrl | 62:d502889e74f1 | 32 | setFilterFrequency(_filterFrequency); |
tnhnrl | 62:d502889e74f1 | 33 | setControllerP(_Pgain); |
tnhnrl | 62:d502889e74f1 | 34 | setControllerI(_Igain); |
tnhnrl | 62:d502889e74f1 | 35 | setControllerD(_Dgain); |
tnhnrl | 62:d502889e74f1 | 36 | |
tnhnrl | 62:d502889e74f1 | 37 | // setup the controller object |
tnhnrl | 62:d502889e74f1 | 38 | toggleDeadband(true); |
tnhnrl | 62:d502889e74f1 | 39 | setDeadband(_deadband); |
tnhnrl | 62:d502889e74f1 | 40 | } |
tnhnrl | 62:d502889e74f1 | 41 | |
tnhnrl | 62:d502889e74f1 | 42 | // attaches the update function to an interval timer |
tnhnrl | 62:d502889e74f1 | 43 | void OuterLoop::start() { |
tnhnrl | 62:d502889e74f1 | 44 | _pulse.attach(this,&OuterLoop::update, _dt); |
tnhnrl | 62:d502889e74f1 | 45 | } |
tnhnrl | 62:d502889e74f1 | 46 | |
tnhnrl | 62:d502889e74f1 | 47 | // detaches the update function from the interval timer |
tnhnrl | 62:d502889e74f1 | 48 | void OuterLoop::stop() { |
tnhnrl | 62:d502889e74f1 | 49 | _pulse.detach(); |
tnhnrl | 62:d502889e74f1 | 50 | } |
tnhnrl | 62:d502889e74f1 | 51 | |
tnhnrl | 62:d502889e74f1 | 52 | void OuterLoop::runOuterLoop() { |
tnhnrl | 62:d502889e74f1 | 53 | update(); |
tnhnrl | 62:d502889e74f1 | 54 | } |
tnhnrl | 62:d502889e74f1 | 55 | |
tnhnrl | 62:d502889e74f1 | 56 | // todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this |
tnhnrl | 62:d502889e74f1 | 57 | void OuterLoop::update() { |
tnhnrl | 62:d502889e74f1 | 58 | // update the position velocity filter |
tnhnrl | 62:d502889e74f1 | 59 | if (_sensor == 0) { |
joel_ssc | 85:dd8176285b6e | 60 | // _sensorVal = depth().getDepthFt(); // change to getting meters |
joel_ssc | 85:dd8176285b6e | 61 | _sensorVal = depth().getDepthM(); |
tnhnrl | 62:d502889e74f1 | 62 | } else if (_sensor == 1) { |
tnhnrl | 62:d502889e74f1 | 63 | _sensorVal = imu().getPitch(); |
tnhnrl | 62:d502889e74f1 | 64 | } else if (_sensor == 2) { |
tnhnrl | 62:d502889e74f1 | 65 | _sensorVal = imu().getHeading(); |
joel_ssc | 87:6d95f853dab3 | 66 | } else if (_sensor == 3) { |
CodyMarquardt | 99:9d0849f5fcd7 | 67 | _sensorVal = sensors().getAltimeterReading_m(); // CAM changed from counts to meters |
joel_ssc | 87:6d95f853dab3 | 68 | // _sensorVal = imu().getPitch(); // 27mar2019 testing sensors error? |
tnhnrl | 62:d502889e74f1 | 69 | } else { |
tnhnrl | 62:d502889e74f1 | 70 | error("\n\r This sensor option does not exist"); |
tnhnrl | 62:d502889e74f1 | 71 | } |
tnhnrl | 62:d502889e74f1 | 72 | |
tnhnrl | 62:d502889e74f1 | 73 | // use the sensor reading to update the PVF |
tnhnrl | 62:d502889e74f1 | 74 | _filter.update(_dt, _sensorVal); |
tnhnrl | 62:d502889e74f1 | 75 | |
tnhnrl | 62:d502889e74f1 | 76 | // refresh the filter results and load into class variables |
tnhnrl | 62:d502889e74f1 | 77 | refreshPVState(); |
tnhnrl | 62:d502889e74f1 | 78 | |
tnhnrl | 62:d502889e74f1 | 79 | // update the PID controller with latest data |
tnhnrl | 62:d502889e74f1 | 80 | _pid.update(_position, _velocity, _filter.getDt()); |
tnhnrl | 62:d502889e74f1 | 81 | } |
tnhnrl | 62:d502889e74f1 | 82 | |
tnhnrl | 62:d502889e74f1 | 83 | void OuterLoop::setCommand(float value) { |
tnhnrl | 62:d502889e74f1 | 84 | _SetPoint = value; |
tnhnrl | 62:d502889e74f1 | 85 | _pid.writeSetPoint(_SetPoint); |
tnhnrl | 62:d502889e74f1 | 86 | } |
tnhnrl | 62:d502889e74f1 | 87 | |
tnhnrl | 62:d502889e74f1 | 88 | float OuterLoop::getCommand() { |
tnhnrl | 62:d502889e74f1 | 89 | return _SetPoint; |
tnhnrl | 62:d502889e74f1 | 90 | } |
tnhnrl | 62:d502889e74f1 | 91 | |
tnhnrl | 62:d502889e74f1 | 92 | float OuterLoop::getOutput() { |
tnhnrl | 62:d502889e74f1 | 93 | /* PID output + offset to drive the motors to the correct position */ |
tnhnrl | 62:d502889e74f1 | 94 | return _pid.getOutput() + _offset; |
tnhnrl | 62:d502889e74f1 | 95 | } |
tnhnrl | 62:d502889e74f1 | 96 | |
tnhnrl | 62:d502889e74f1 | 97 | void OuterLoop::refreshPVState() { |
tnhnrl | 62:d502889e74f1 | 98 | _position = _filter.getPosition(); |
tnhnrl | 62:d502889e74f1 | 99 | _velocity = _filter.getVelocity(); |
tnhnrl | 62:d502889e74f1 | 100 | } |
tnhnrl | 62:d502889e74f1 | 101 | |
tnhnrl | 62:d502889e74f1 | 102 | float OuterLoop::getPosition() { |
tnhnrl | 62:d502889e74f1 | 103 | return _position; |
tnhnrl | 62:d502889e74f1 | 104 | } |
tnhnrl | 62:d502889e74f1 | 105 | |
tnhnrl | 62:d502889e74f1 | 106 | float OuterLoop::getVelocity() { |
tnhnrl | 62:d502889e74f1 | 107 | return _velocity; |
tnhnrl | 62:d502889e74f1 | 108 | } |
tnhnrl | 62:d502889e74f1 | 109 | |
tnhnrl | 62:d502889e74f1 | 110 | void OuterLoop::setControllerP(float P) { |
tnhnrl | 62:d502889e74f1 | 111 | _Pgain = P; |
tnhnrl | 62:d502889e74f1 | 112 | _pid.setPgain(_Pgain); |
tnhnrl | 62:d502889e74f1 | 113 | } |
tnhnrl | 62:d502889e74f1 | 114 | |
tnhnrl | 62:d502889e74f1 | 115 | float OuterLoop::getControllerP() { |
tnhnrl | 62:d502889e74f1 | 116 | return _Pgain; |
tnhnrl | 62:d502889e74f1 | 117 | } |
tnhnrl | 62:d502889e74f1 | 118 | |
tnhnrl | 62:d502889e74f1 | 119 | void OuterLoop::setControllerI(float I) { |
tnhnrl | 62:d502889e74f1 | 120 | _Igain = I; |
tnhnrl | 62:d502889e74f1 | 121 | _pid.setIgain(_Igain); |
tnhnrl | 62:d502889e74f1 | 122 | } |
tnhnrl | 62:d502889e74f1 | 123 | |
tnhnrl | 62:d502889e74f1 | 124 | float OuterLoop::getControllerI() { |
tnhnrl | 62:d502889e74f1 | 125 | return _Igain; |
tnhnrl | 62:d502889e74f1 | 126 | } |
tnhnrl | 62:d502889e74f1 | 127 | |
tnhnrl | 62:d502889e74f1 | 128 | void OuterLoop::setControllerD(float D) { |
tnhnrl | 62:d502889e74f1 | 129 | _Dgain = D; |
tnhnrl | 62:d502889e74f1 | 130 | _pid.setDgain(_Dgain); |
tnhnrl | 62:d502889e74f1 | 131 | } |
tnhnrl | 62:d502889e74f1 | 132 | |
tnhnrl | 62:d502889e74f1 | 133 | float OuterLoop::getControllerD() { |
tnhnrl | 62:d502889e74f1 | 134 | return _Dgain; |
tnhnrl | 62:d502889e74f1 | 135 | } |
tnhnrl | 73:f6f378311c8d | 136 | |
tnhnrl | 73:f6f378311c8d | 137 | //uses position velocity filter class |
tnhnrl | 62:d502889e74f1 | 138 | void OuterLoop::setFilterFrequency(float frequency) { |
tnhnrl | 62:d502889e74f1 | 139 | _filterFrequency = frequency; |
tnhnrl | 62:d502889e74f1 | 140 | _filter.writeWn(frequency); |
tnhnrl | 62:d502889e74f1 | 141 | } |
tnhnrl | 73:f6f378311c8d | 142 | |
tnhnrl | 73:f6f378311c8d | 143 | float OuterLoop::getFilterFrequency() { |
tnhnrl | 73:f6f378311c8d | 144 | return _filterFrequency; //new 7/11/18 |
tnhnrl | 73:f6f378311c8d | 145 | } |
tnhnrl | 62:d502889e74f1 | 146 | |
tnhnrl | 62:d502889e74f1 | 147 | void OuterLoop::setDeadband(float deadband) { |
tnhnrl | 62:d502889e74f1 | 148 | _deadband = deadband; |
tnhnrl | 62:d502889e74f1 | 149 | _pid.setDeadBand(_deadband); |
tnhnrl | 62:d502889e74f1 | 150 | } |
tnhnrl | 62:d502889e74f1 | 151 | |
tnhnrl | 62:d502889e74f1 | 152 | float OuterLoop::getDeadband() { |
tnhnrl | 62:d502889e74f1 | 153 | return _deadband; |
tnhnrl | 62:d502889e74f1 | 154 | } |
tnhnrl | 62:d502889e74f1 | 155 | |
tnhnrl | 62:d502889e74f1 | 156 | bool OuterLoop::toggleDeadband(bool toggle) { |
tnhnrl | 62:d502889e74f1 | 157 | _pid.toggleDeadBand(toggle); |
tnhnrl | 62:d502889e74f1 | 158 | return toggle; |
tnhnrl | 62:d502889e74f1 | 159 | } |
tnhnrl | 62:d502889e74f1 | 160 | |
tnhnrl | 62:d502889e74f1 | 161 | void OuterLoop::setOutputOffset(float offset) { |
tnhnrl | 62:d502889e74f1 | 162 | _offset = offset; |
tnhnrl | 62:d502889e74f1 | 163 | } |
tnhnrl | 62:d502889e74f1 | 164 | |
tnhnrl | 62:d502889e74f1 | 165 | float OuterLoop::getOutputOffset() { |
tnhnrl | 62:d502889e74f1 | 166 | return _offset; |
tnhnrl | 62:d502889e74f1 | 167 | } |
tnhnrl | 62:d502889e74f1 | 168 | |
tnhnrl | 62:d502889e74f1 | 169 | //not used yet |
tnhnrl | 62:d502889e74f1 | 170 | void OuterLoop::setIHiLimit (float limit){ |
tnhnrl | 62:d502889e74f1 | 171 | _pid.setHiLimit(limit); |
tnhnrl | 62:d502889e74f1 | 172 | } |
tnhnrl | 62:d502889e74f1 | 173 | |
tnhnrl | 62:d502889e74f1 | 174 | void OuterLoop::setILoLimit (float limit){ |
tnhnrl | 62:d502889e74f1 | 175 | _pid.setHiLimit(limit); |
tnhnrl | 62:d502889e74f1 | 176 | } |