Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

OuterLoop/OuterLoop.cpp@92:52a91656458a, 2019-05-13 (annotated)

Committer:
joel_ssc
Date:
Mon May 13 19:25:26 2019 +0000
Revision:
92:52a91656458a
Parent:
87:6d95f853dab3 
version for first flight test, timeouts not yet set correctly

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 87:6d95f853dab3 67 _sensorVal = sensors().getAltimeterChannelReadings(); // 27mar2019 this is still in counts
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 }