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@55:f4ec445c42fe, 2018-06-12 (annotated)

Committer:
tnhnrl
Date:
Tue Jun 12 15:41:51 2018 +0000
Revision:
55:f4ec445c42fe
Working v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 55:f4ec445c42fe 1 #include "mbed.h"
tnhnrl 55:f4ec445c42fe 2 #include "OuterLoop.hpp"
tnhnrl 55:f4ec445c42fe 3 #include "StaticDefs.hpp"
tnhnrl 55:f4ec445c42fe 4
tnhnrl 55:f4ec445c42fe 5 // this is where the variables that can be set are set when the object is created
tnhnrl 55:f4ec445c42fe 6 OuterLoop::OuterLoop(float interval, int sensor):
tnhnrl 55:f4ec445c42fe 7 _filter(),
tnhnrl 55:f4ec445c42fe 8 _pid(),
tnhnrl 55:f4ec445c42fe 9 _pulse()
tnhnrl 55:f4ec445c42fe 10 {
tnhnrl 55:f4ec445c42fe 11 _Pgain = 0.5;
tnhnrl 55:f4ec445c42fe 12 _Igain = 0.0;
tnhnrl 55:f4ec445c42fe 13 _Dgain = 0.1;
tnhnrl 55:f4ec445c42fe 14
tnhnrl 55:f4ec445c42fe 15 _filterFrequency = 2.0;
tnhnrl 55:f4ec445c42fe 16 _deadband = 0.0;
tnhnrl 55:f4ec445c42fe 17
tnhnrl 55:f4ec445c42fe 18 _dt = interval;
tnhnrl 55:f4ec445c42fe 19 _sensor = sensor; // select the sensor ... hacky
tnhnrl 55:f4ec445c42fe 20 }
tnhnrl 55:f4ec445c42fe 21
tnhnrl 55:f4ec445c42fe 22 // initialize and start all of the member objects.
tnhnrl 55:f4ec445c42fe 23 void OuterLoop::init() {
tnhnrl 55:f4ec445c42fe 24 // load gains into pid controller this should eventually be in a config file
tnhnrl 55:f4ec445c42fe 25 setFilterFrequency(_filterFrequency);
tnhnrl 55:f4ec445c42fe 26 setControllerP(_Pgain);
tnhnrl 55:f4ec445c42fe 27 setControllerI(_Igain);
tnhnrl 55:f4ec445c42fe 28 setControllerD(_Dgain);
tnhnrl 55:f4ec445c42fe 29
tnhnrl 55:f4ec445c42fe 30 // setup the controller object
tnhnrl 55:f4ec445c42fe 31 toggleDeadband(true);
tnhnrl 55:f4ec445c42fe 32 setDeadband(_deadband);
tnhnrl 55:f4ec445c42fe 33 }
tnhnrl 55:f4ec445c42fe 34
tnhnrl 55:f4ec445c42fe 35 // attaches the update function to an interval timer
tnhnrl 55:f4ec445c42fe 36 void OuterLoop::start() {
tnhnrl 55:f4ec445c42fe 37 _pulse.attach(this,&OuterLoop::update, _dt);
tnhnrl 55:f4ec445c42fe 38 }
tnhnrl 55:f4ec445c42fe 39
tnhnrl 55:f4ec445c42fe 40 // detaches the update function from the interval timer
tnhnrl 55:f4ec445c42fe 41 void OuterLoop::stop() {
tnhnrl 55:f4ec445c42fe 42 _pulse.detach();
tnhnrl 55:f4ec445c42fe 43 }
tnhnrl 55:f4ec445c42fe 44
tnhnrl 55:f4ec445c42fe 45 // todo: these sensor reads should be somehow attached as callbacks, not hard-coded like this
tnhnrl 55:f4ec445c42fe 46 void OuterLoop::update() {
tnhnrl 55:f4ec445c42fe 47 // update the position velocity filter
tnhnrl 55:f4ec445c42fe 48 if (_sensor == 0) {
tnhnrl 55:f4ec445c42fe 49 _sensorVal = depth().getDepthFt();
tnhnrl 55:f4ec445c42fe 50 } else if (_sensor == 1) {
tnhnrl 55:f4ec445c42fe 51 _sensorVal = imu().getPitch();
tnhnrl 55:f4ec445c42fe 52 } else if (_sensor == 2) {
tnhnrl 55:f4ec445c42fe 53 _sensorVal = imu().getHeading();
tnhnrl 55:f4ec445c42fe 54 } else {
tnhnrl 55:f4ec445c42fe 55 error("\n\r This sensor option does not exist");
tnhnrl 55:f4ec445c42fe 56 }
tnhnrl 55:f4ec445c42fe 57
tnhnrl 55:f4ec445c42fe 58 // use the sensor reading to update the PVF
tnhnrl 55:f4ec445c42fe 59 _filter.update(_dt, _sensorVal);
tnhnrl 55:f4ec445c42fe 60
tnhnrl 55:f4ec445c42fe 61 // refresh the filter results and load into class variables
tnhnrl 55:f4ec445c42fe 62 refreshPVState();
tnhnrl 55:f4ec445c42fe 63
tnhnrl 55:f4ec445c42fe 64 // update the PID controller with latest data
tnhnrl 55:f4ec445c42fe 65 _pid.update(_position, _velocity, _filter.getDt());
tnhnrl 55:f4ec445c42fe 66 }
tnhnrl 55:f4ec445c42fe 67
tnhnrl 55:f4ec445c42fe 68 void OuterLoop::setCommand(float value) {
tnhnrl 55:f4ec445c42fe 69 _SetPoint = value;
tnhnrl 55:f4ec445c42fe 70 _pid.writeSetPoint(_SetPoint);
tnhnrl 55:f4ec445c42fe 71 }
tnhnrl 55:f4ec445c42fe 72
tnhnrl 55:f4ec445c42fe 73 float OuterLoop::getCommand() {
tnhnrl 55:f4ec445c42fe 74 return _SetPoint;
tnhnrl 55:f4ec445c42fe 75 }
tnhnrl 55:f4ec445c42fe 76
tnhnrl 55:f4ec445c42fe 77 float OuterLoop::getOutput() {
tnhnrl 55:f4ec445c42fe 78 /* PID output + offset to drive the motors to the correct position */
tnhnrl 55:f4ec445c42fe 79 return _pid.getOutput() + _offset;
tnhnrl 55:f4ec445c42fe 80 }
tnhnrl 55:f4ec445c42fe 81
tnhnrl 55:f4ec445c42fe 82 void OuterLoop::refreshPVState() {
tnhnrl 55:f4ec445c42fe 83 _position = _filter.getPosition();
tnhnrl 55:f4ec445c42fe 84 _velocity = _filter.getVelocity();
tnhnrl 55:f4ec445c42fe 85 }
tnhnrl 55:f4ec445c42fe 86
tnhnrl 55:f4ec445c42fe 87 float OuterLoop::getPosition() {
tnhnrl 55:f4ec445c42fe 88 return _position;
tnhnrl 55:f4ec445c42fe 89 }
tnhnrl 55:f4ec445c42fe 90
tnhnrl 55:f4ec445c42fe 91 float OuterLoop::getVelocity() {
tnhnrl 55:f4ec445c42fe 92 return _velocity;
tnhnrl 55:f4ec445c42fe 93 }
tnhnrl 55:f4ec445c42fe 94
tnhnrl 55:f4ec445c42fe 95 void OuterLoop::setControllerP(float P) {
tnhnrl 55:f4ec445c42fe 96 _Pgain = P;
tnhnrl 55:f4ec445c42fe 97 _pid.setPgain(_Pgain);
tnhnrl 55:f4ec445c42fe 98 }
tnhnrl 55:f4ec445c42fe 99
tnhnrl 55:f4ec445c42fe 100 float OuterLoop::getControllerP() {
tnhnrl 55:f4ec445c42fe 101 return _Pgain;
tnhnrl 55:f4ec445c42fe 102 }
tnhnrl 55:f4ec445c42fe 103
tnhnrl 55:f4ec445c42fe 104 void OuterLoop::setControllerI(float I) {
tnhnrl 55:f4ec445c42fe 105 _Igain = I;
tnhnrl 55:f4ec445c42fe 106 _pid.setIgain(_Igain);
tnhnrl 55:f4ec445c42fe 107 }
tnhnrl 55:f4ec445c42fe 108
tnhnrl 55:f4ec445c42fe 109 float OuterLoop::getControllerI() {
tnhnrl 55:f4ec445c42fe 110 return _Igain;
tnhnrl 55:f4ec445c42fe 111 }
tnhnrl 55:f4ec445c42fe 112
tnhnrl 55:f4ec445c42fe 113 void OuterLoop::setControllerD(float D) {
tnhnrl 55:f4ec445c42fe 114 _Dgain = D;
tnhnrl 55:f4ec445c42fe 115 _pid.setDgain(_Dgain);
tnhnrl 55:f4ec445c42fe 116 }
tnhnrl 55:f4ec445c42fe 117
tnhnrl 55:f4ec445c42fe 118 float OuterLoop::getControllerD() {
tnhnrl 55:f4ec445c42fe 119 return _Dgain;
tnhnrl 55:f4ec445c42fe 120 }
tnhnrl 55:f4ec445c42fe 121
tnhnrl 55:f4ec445c42fe 122 void OuterLoop::setFilterFrequency(float frequency) {
tnhnrl 55:f4ec445c42fe 123 _filterFrequency = frequency;
tnhnrl 55:f4ec445c42fe 124 _filter.writeWn(frequency);
tnhnrl 55:f4ec445c42fe 125 }
tnhnrl 55:f4ec445c42fe 126
tnhnrl 55:f4ec445c42fe 127 void OuterLoop::setDeadband(float deadband) {
tnhnrl 55:f4ec445c42fe 128 _deadband = deadband;
tnhnrl 55:f4ec445c42fe 129 _pid.setDeadBand(_deadband);
tnhnrl 55:f4ec445c42fe 130 }
tnhnrl 55:f4ec445c42fe 131
tnhnrl 55:f4ec445c42fe 132 bool OuterLoop::toggleDeadband(bool toggle) {
tnhnrl 55:f4ec445c42fe 133 _pid.toggleDeadBand(toggle);
tnhnrl 55:f4ec445c42fe 134 return toggle;
tnhnrl 55:f4ec445c42fe 135 }
tnhnrl 55:f4ec445c42fe 136
tnhnrl 55:f4ec445c42fe 137 void OuterLoop::setOutputOffset(float offset) {
tnhnrl 55:f4ec445c42fe 138 _offset = offset;
tnhnrl 55:f4ec445c42fe 139 }
tnhnrl 55:f4ec445c42fe 140
tnhnrl 55:f4ec445c42fe 141 float OuterLoop::getOutputOffset() {
tnhnrl 55:f4ec445c42fe 142 return _offset;
tnhnrl 55:f4ec445c42fe 143 }