Flying Sea Glider / Mbed 2 deprecated 2019_13sep_jcw_nosd

update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()

Dependencies:   mbed MODSERIAL FATFileSystem

OuterLoop/OuterLoop.cpp@104:426224a55f5f, 2019-09-13 (annotated)

Committer:
joel_ssc
Date:
Fri Sep 13 19:15:40 2019 +0000
Revision:
104:426224a55f5f
Parent:
102:0f430de62447 
slight change  includes reset_PI

Who changed what in which revision?

UserRevisionLine numberNew 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) {
joel_ssc 102:0f430de62447 67 // _sensorVal = sensors().getAltimeterChannelReadings(); // 27mar2019 this is still in counts
joel_ssc 87:6d95f853dab3 68 // _sensorVal = imu().getPitch(); // 27mar2019 testing sensors error?
joel_ssc 102:0f430de62447 69 //} else if (_sensor == 3) {
joel_ssc 102:0f430de62447 70 _sensorVal = sensors().getAltimeterReading_m(); // CAM changed from counts to meters
tnhnrl 62:d502889e74f1 71 } else {
tnhnrl 62:d502889e74f1 72 error("\n\r This sensor option does not exist");
tnhnrl 62:d502889e74f1 73 }
tnhnrl 62:d502889e74f1 74
tnhnrl 62:d502889e74f1 75 // use the sensor reading to update the PVF
tnhnrl 62:d502889e74f1 76 _filter.update(_dt, _sensorVal);
tnhnrl 62:d502889e74f1 77
tnhnrl 62:d502889e74f1 78 // refresh the filter results and load into class variables
tnhnrl 62:d502889e74f1 79 refreshPVState();
tnhnrl 62:d502889e74f1 80
tnhnrl 62:d502889e74f1 81 // update the PID controller with latest data
tnhnrl 62:d502889e74f1 82 _pid.update(_position, _velocity, _filter.getDt());
tnhnrl 62:d502889e74f1 83 }
tnhnrl 62:d502889e74f1 84
tnhnrl 62:d502889e74f1 85 void OuterLoop::setCommand(float value) {
tnhnrl 62:d502889e74f1 86 _SetPoint = value;
tnhnrl 62:d502889e74f1 87 _pid.writeSetPoint(_SetPoint);
tnhnrl 62:d502889e74f1 88 }
tnhnrl 62:d502889e74f1 89
tnhnrl 62:d502889e74f1 90 float OuterLoop::getCommand() {
tnhnrl 62:d502889e74f1 91 return _SetPoint;
tnhnrl 62:d502889e74f1 92 }
tnhnrl 62:d502889e74f1 93
tnhnrl 62:d502889e74f1 94 float OuterLoop::getOutput() {
tnhnrl 62:d502889e74f1 95 /* PID output + offset to drive the motors to the correct position */
tnhnrl 62:d502889e74f1 96 return _pid.getOutput() + _offset;
tnhnrl 62:d502889e74f1 97 }
tnhnrl 62:d502889e74f1 98
tnhnrl 62:d502889e74f1 99 void OuterLoop::refreshPVState() {
tnhnrl 62:d502889e74f1 100 _position = _filter.getPosition();
tnhnrl 62:d502889e74f1 101 _velocity = _filter.getVelocity();
tnhnrl 62:d502889e74f1 102 }
tnhnrl 62:d502889e74f1 103
tnhnrl 62:d502889e74f1 104 float OuterLoop::getPosition() {
tnhnrl 62:d502889e74f1 105 return _position;
tnhnrl 62:d502889e74f1 106 }
tnhnrl 62:d502889e74f1 107
tnhnrl 62:d502889e74f1 108 float OuterLoop::getVelocity() {
tnhnrl 62:d502889e74f1 109 return _velocity;
tnhnrl 62:d502889e74f1 110 }
tnhnrl 62:d502889e74f1 111
tnhnrl 62:d502889e74f1 112 void OuterLoop::setControllerP(float P) {
tnhnrl 62:d502889e74f1 113 _Pgain = P;
tnhnrl 62:d502889e74f1 114 _pid.setPgain(_Pgain);
tnhnrl 62:d502889e74f1 115 }
tnhnrl 62:d502889e74f1 116
tnhnrl 62:d502889e74f1 117 float OuterLoop::getControllerP() {
tnhnrl 62:d502889e74f1 118 return _Pgain;
tnhnrl 62:d502889e74f1 119 }
tnhnrl 62:d502889e74f1 120
tnhnrl 62:d502889e74f1 121 void OuterLoop::setControllerI(float I) {
tnhnrl 62:d502889e74f1 122 _Igain = I;
tnhnrl 62:d502889e74f1 123 _pid.setIgain(_Igain);
tnhnrl 62:d502889e74f1 124 }
tnhnrl 62:d502889e74f1 125
tnhnrl 62:d502889e74f1 126 float OuterLoop::getControllerI() {
tnhnrl 62:d502889e74f1 127 return _Igain;
tnhnrl 62:d502889e74f1 128 }
tnhnrl 62:d502889e74f1 129
tnhnrl 62:d502889e74f1 130 void OuterLoop::setControllerD(float D) {
tnhnrl 62:d502889e74f1 131 _Dgain = D;
tnhnrl 62:d502889e74f1 132 _pid.setDgain(_Dgain);
tnhnrl 62:d502889e74f1 133 }
tnhnrl 62:d502889e74f1 134
tnhnrl 62:d502889e74f1 135 float OuterLoop::getControllerD() {
tnhnrl 62:d502889e74f1 136 return _Dgain;
tnhnrl 62:d502889e74f1 137 }
tnhnrl 73:f6f378311c8d 138
tnhnrl 73:f6f378311c8d 139 //uses position velocity filter class
tnhnrl 62:d502889e74f1 140 void OuterLoop::setFilterFrequency(float frequency) {
tnhnrl 62:d502889e74f1 141 _filterFrequency = frequency;
tnhnrl 62:d502889e74f1 142 _filter.writeWn(frequency);
tnhnrl 62:d502889e74f1 143 }
tnhnrl 73:f6f378311c8d 144
tnhnrl 73:f6f378311c8d 145 float OuterLoop::getFilterFrequency() {
tnhnrl 73:f6f378311c8d 146 return _filterFrequency; //new 7/11/18
tnhnrl 73:f6f378311c8d 147 }
tnhnrl 62:d502889e74f1 148
tnhnrl 62:d502889e74f1 149 void OuterLoop::setDeadband(float deadband) {
tnhnrl 62:d502889e74f1 150 _deadband = deadband;
tnhnrl 62:d502889e74f1 151 _pid.setDeadBand(_deadband);
tnhnrl 62:d502889e74f1 152 }
tnhnrl 62:d502889e74f1 153
tnhnrl 62:d502889e74f1 154 float OuterLoop::getDeadband() {
tnhnrl 62:d502889e74f1 155 return _deadband;
tnhnrl 62:d502889e74f1 156 }
tnhnrl 62:d502889e74f1 157
tnhnrl 62:d502889e74f1 158 bool OuterLoop::toggleDeadband(bool toggle) {
tnhnrl 62:d502889e74f1 159 _pid.toggleDeadBand(toggle);
tnhnrl 62:d502889e74f1 160 return toggle;
tnhnrl 62:d502889e74f1 161 }
tnhnrl 62:d502889e74f1 162
tnhnrl 62:d502889e74f1 163 void OuterLoop::setOutputOffset(float offset) {
tnhnrl 62:d502889e74f1 164 _offset = offset;
tnhnrl 62:d502889e74f1 165 }
tnhnrl 62:d502889e74f1 166
tnhnrl 62:d502889e74f1 167 float OuterLoop::getOutputOffset() {
tnhnrl 62:d502889e74f1 168 return _offset;
tnhnrl 62:d502889e74f1 169 }
tnhnrl 62:d502889e74f1 170
tnhnrl 62:d502889e74f1 171 //not used yet
tnhnrl 62:d502889e74f1 172 void OuterLoop::setIHiLimit (float limit){
tnhnrl 62:d502889e74f1 173 _pid.setHiLimit(limit);
tnhnrl 62:d502889e74f1 174 }
tnhnrl 62:d502889e74f1 175
tnhnrl 62:d502889e74f1 176 void OuterLoop::setILoLimit (float limit){
tnhnrl 62:d502889e74f1 177 _pid.setHiLimit(limit);
tnhnrl 62:d502889e74f1 178 }