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@85:dd8176285b6e, 2019-02-25 (annotated)

Committer:
joel_ssc
Date:
Mon Feb 25 21:26:34 2019 +0000
Revision:
85:dd8176285b6e
Parent:
74:d281aaef9766
Child:
87:6d95f853dab3
tests imu.roll

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