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@14:85b64a4d08e8, 2017-10-31 (annotated)

Committer:
danstrider
Date:
Tue Oct 31 17:06:52 2017 +0000
Revision:
14:85b64a4d08e8
Parent:
11:3b241ecb75ed 
New depth sensor class with tare and oversampling.

Who changed what in which revision?

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