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@11:3b241ecb75ed, 2017-10-27 (annotated)

Committer:
danstrider
Date:
Fri Oct 27 00:37:32 2017 +0000
Revision:
11:3b241ecb75ed
Parent:
10:085ab7328054
Child:
14:85b64a4d08e8
This version has been in the pool, working with all the hardware.  Had occasional string pot problems and got stuck in RISE, letting the battery stall out against the endcap.

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 10:085ab7328054 49 _sensorVal = depth().getDepth();
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 }