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@74:d281aaef9766, 2018-08-14 (annotated)

Committer:
tnhnrl
Date:
Tue Aug 14 21:06:48 2018 +0000
Revision:
74:d281aaef9766
Parent:
73:f6f378311c8d
Child:
85:dd8176285b6e
1) Fixed deadband on float broadcast ; 2) Fixed find neutral commands

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
tnhnrl 62:d502889e74f1 25 }
tnhnrl 62:d502889e74f1 26 }
tnhnrl 62:d502889e74f1 27
tnhnrl 62:d502889e74f1 28 // initialize and start all of the member objects.
tnhnrl 62:d502889e74f1 29 void OuterLoop::init() {
tnhnrl 62:d502889e74f1 30 // load gains into pid controller this should eventually be in a config file
tnhnrl 62:d502889e74f1 31 setFilterFrequency(_filterFrequency);
tnhnrl 62:d502889e74f1 32 setControllerP(_Pgain);
tnhnrl 62:d502889e74f1 33 setControllerI(_Igain);
tnhnrl 62:d502889e74f1 34 setControllerD(_Dgain);
tnhnrl 62:d502889e74f1 35
tnhnrl 62:d502889e74f1 36 // setup the controller object
tnhnrl 62:d502889e74f1 37 toggleDeadband(true);
tnhnrl 62:d502889e74f1 38 setDeadband(_deadband);
tnhnrl 62:d502889e74f1 39 }
tnhnrl 62:d502889e74f1 40
tnhnrl 62:d502889e74f1 41 // attaches the update function to an interval timer
tnhnrl 62:d502889e74f1 42 void OuterLoop::start() {
tnhnrl 62:d502889e74f1 43 _pulse.attach(this,&OuterLoop::update, _dt);
tnhnrl 62:d502889e74f1 44 }
tnhnrl 62:d502889e74f1 45
tnhnrl 62:d502889e74f1 46 // detaches the update function from the interval timer
tnhnrl 62:d502889e74f1 47 void OuterLoop::stop() {
tnhnrl 62:d502889e74f1 48 _pulse.detach();
tnhnrl 62:d502889e74f1 49 }
tnhnrl 62:d502889e74f1 50
tnhnrl 62:d502889e74f1 51 void OuterLoop::runOuterLoop() {
tnhnrl 62:d502889e74f1 52 update();
tnhnrl 62:d502889e74f1 53 }
tnhnrl 62:d502889e74f1 54
tnhnrl 62:d502889e74f1 55 // todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this
tnhnrl 62:d502889e74f1 56 void OuterLoop::update() {
tnhnrl 62:d502889e74f1 57 // update the position velocity filter
tnhnrl 62:d502889e74f1 58 if (_sensor == 0) {
tnhnrl 62:d502889e74f1 59 _sensorVal = depth().getDepthFt();
tnhnrl 62:d502889e74f1 60 } else if (_sensor == 1) {
tnhnrl 62:d502889e74f1 61 _sensorVal = imu().getPitch();
tnhnrl 62:d502889e74f1 62 } else if (_sensor == 2) {
tnhnrl 62:d502889e74f1 63 _sensorVal = imu().getHeading();
tnhnrl 62:d502889e74f1 64 } else {
tnhnrl 62:d502889e74f1 65 error("\n\r This sensor option does not exist");
tnhnrl 62:d502889e74f1 66 }
tnhnrl 62:d502889e74f1 67
tnhnrl 62:d502889e74f1 68 // use the sensor reading to update the PVF
tnhnrl 62:d502889e74f1 69 _filter.update(_dt, _sensorVal);
tnhnrl 62:d502889e74f1 70
tnhnrl 62:d502889e74f1 71 // refresh the filter results and load into class variables
tnhnrl 62:d502889e74f1 72 refreshPVState();
tnhnrl 62:d502889e74f1 73
tnhnrl 62:d502889e74f1 74 // update the PID controller with latest data
tnhnrl 62:d502889e74f1 75 _pid.update(_position, _velocity, _filter.getDt());
tnhnrl 62:d502889e74f1 76 }
tnhnrl 62:d502889e74f1 77
tnhnrl 62:d502889e74f1 78 void OuterLoop::setCommand(float value) {
tnhnrl 62:d502889e74f1 79 _SetPoint = value;
tnhnrl 62:d502889e74f1 80 _pid.writeSetPoint(_SetPoint);
tnhnrl 62:d502889e74f1 81 }
tnhnrl 62:d502889e74f1 82
tnhnrl 62:d502889e74f1 83 float OuterLoop::getCommand() {
tnhnrl 62:d502889e74f1 84 return _SetPoint;
tnhnrl 62:d502889e74f1 85 }
tnhnrl 62:d502889e74f1 86
tnhnrl 62:d502889e74f1 87 float OuterLoop::getOutput() {
tnhnrl 62:d502889e74f1 88 /* PID output + offset to drive the motors to the correct position */
tnhnrl 62:d502889e74f1 89 return _pid.getOutput() + _offset;
tnhnrl 62:d502889e74f1 90 }
tnhnrl 62:d502889e74f1 91
tnhnrl 62:d502889e74f1 92 void OuterLoop::refreshPVState() {
tnhnrl 62:d502889e74f1 93 _position = _filter.getPosition();
tnhnrl 62:d502889e74f1 94 _velocity = _filter.getVelocity();
tnhnrl 62:d502889e74f1 95 }
tnhnrl 62:d502889e74f1 96
tnhnrl 62:d502889e74f1 97 float OuterLoop::getPosition() {
tnhnrl 62:d502889e74f1 98 return _position;
tnhnrl 62:d502889e74f1 99 }
tnhnrl 62:d502889e74f1 100
tnhnrl 62:d502889e74f1 101 float OuterLoop::getVelocity() {
tnhnrl 62:d502889e74f1 102 return _velocity;
tnhnrl 62:d502889e74f1 103 }
tnhnrl 62:d502889e74f1 104
tnhnrl 62:d502889e74f1 105 void OuterLoop::setControllerP(float P) {
tnhnrl 62:d502889e74f1 106 _Pgain = P;
tnhnrl 62:d502889e74f1 107 _pid.setPgain(_Pgain);
tnhnrl 62:d502889e74f1 108 }
tnhnrl 62:d502889e74f1 109
tnhnrl 62:d502889e74f1 110 float OuterLoop::getControllerP() {
tnhnrl 62:d502889e74f1 111 return _Pgain;
tnhnrl 62:d502889e74f1 112 }
tnhnrl 62:d502889e74f1 113
tnhnrl 62:d502889e74f1 114 void OuterLoop::setControllerI(float I) {
tnhnrl 62:d502889e74f1 115 _Igain = I;
tnhnrl 62:d502889e74f1 116 _pid.setIgain(_Igain);
tnhnrl 62:d502889e74f1 117 }
tnhnrl 62:d502889e74f1 118
tnhnrl 62:d502889e74f1 119 float OuterLoop::getControllerI() {
tnhnrl 62:d502889e74f1 120 return _Igain;
tnhnrl 62:d502889e74f1 121 }
tnhnrl 62:d502889e74f1 122
tnhnrl 62:d502889e74f1 123 void OuterLoop::setControllerD(float D) {
tnhnrl 62:d502889e74f1 124 _Dgain = D;
tnhnrl 62:d502889e74f1 125 _pid.setDgain(_Dgain);
tnhnrl 62:d502889e74f1 126 }
tnhnrl 62:d502889e74f1 127
tnhnrl 62:d502889e74f1 128 float OuterLoop::getControllerD() {
tnhnrl 62:d502889e74f1 129 return _Dgain;
tnhnrl 62:d502889e74f1 130 }
tnhnrl 73:f6f378311c8d 131
tnhnrl 73:f6f378311c8d 132 //uses position velocity filter class
tnhnrl 62:d502889e74f1 133 void OuterLoop::setFilterFrequency(float frequency) {
tnhnrl 62:d502889e74f1 134 _filterFrequency = frequency;
tnhnrl 62:d502889e74f1 135 _filter.writeWn(frequency);
tnhnrl 62:d502889e74f1 136 }
tnhnrl 73:f6f378311c8d 137
tnhnrl 73:f6f378311c8d 138 float OuterLoop::getFilterFrequency() {
tnhnrl 73:f6f378311c8d 139 return _filterFrequency; //new 7/11/18
tnhnrl 73:f6f378311c8d 140 }
tnhnrl 62:d502889e74f1 141
tnhnrl 62:d502889e74f1 142 void OuterLoop::setDeadband(float deadband) {
tnhnrl 62:d502889e74f1 143 _deadband = deadband;
tnhnrl 62:d502889e74f1 144 _pid.setDeadBand(_deadband);
tnhnrl 62:d502889e74f1 145 }
tnhnrl 62:d502889e74f1 146
tnhnrl 62:d502889e74f1 147 float OuterLoop::getDeadband() {
tnhnrl 62:d502889e74f1 148 return _deadband;
tnhnrl 62:d502889e74f1 149 }
tnhnrl 62:d502889e74f1 150
tnhnrl 62:d502889e74f1 151 bool OuterLoop::toggleDeadband(bool toggle) {
tnhnrl 62:d502889e74f1 152 _pid.toggleDeadBand(toggle);
tnhnrl 62:d502889e74f1 153 return toggle;
tnhnrl 62:d502889e74f1 154 }
tnhnrl 62:d502889e74f1 155
tnhnrl 62:d502889e74f1 156 void OuterLoop::setOutputOffset(float offset) {
tnhnrl 62:d502889e74f1 157 _offset = offset;
tnhnrl 62:d502889e74f1 158 }
tnhnrl 62:d502889e74f1 159
tnhnrl 62:d502889e74f1 160 float OuterLoop::getOutputOffset() {
tnhnrl 62:d502889e74f1 161 return _offset;
tnhnrl 62:d502889e74f1 162 }
tnhnrl 62:d502889e74f1 163
tnhnrl 62:d502889e74f1 164 //not used yet
tnhnrl 62:d502889e74f1 165 void OuterLoop::setIHiLimit (float limit){
tnhnrl 62:d502889e74f1 166 _pid.setHiLimit(limit);
tnhnrl 62:d502889e74f1 167 }
tnhnrl 62:d502889e74f1 168
tnhnrl 62:d502889e74f1 169 void OuterLoop::setILoLimit (float limit){
tnhnrl 62:d502889e74f1 170 _pid.setHiLimit(limit);
tnhnrl 62:d502889e74f1 171 }