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

main.cpp@10:085ab7328054, 2017-10-23 (annotated)

Committer:
danstrider
Date:
Mon Oct 23 12:50:53 2017 +0000
Revision:
10:085ab7328054
Parent:
5:15bd96205bb2
Child:
11:3b241ecb75ed
checked out on the hardware

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
danstrider 10:085ab7328054 2 Starting from Trent's Linear Actuator code from 2017-10-19, these modifications
danstrider 10:085ab7328054 3 by Dan add an outer loop controller for depth and pitch to command the inner
danstrider 10:085ab7328054 4 linear actuator loops.
danstrider 10:085ab7328054 5 Modified 2017-10-20 revA by Dan
danstrider 10:085ab7328054 6 - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update)
danstrider 10:085ab7328054 7 Modified 2017-10-22 revA by Dan
danstrider 10:085ab7328054 8 - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker
danstrider 10:085ab7328054 9 Modified 2017-10-22 revB by Dan
danstrider 10:085ab7328054 10 - enabled both depth and pitch outer loop controllers
danstrider 10:085ab7328054 11 - added ability to keyboard reset
danstrider 10:085ab7328054 12 Modified 2017-10-22 revC by Dan
danstrider 10:085ab7328054 13 - major update to the IMU library processing to make a state machine that doesn't hang
danstrider 10:085ab7328054 14 - added lat/lon/alt and GNSS fix information to the IMU library
danstrider 10:085ab7328054 15 - brought out the pin names into the constructors of IMU, omega, SpiADC
danstrider 10:085ab7328054 16 Modified 2017-10-22 revD by Dan
danstrider 10:085ab7328054 17 - everything seems to be working, shy of re-checking on the hardware
danstrider 10:085ab7328054 18 - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead
danstrider 10:085ab7328054 19 - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead
danstrider 10:085ab7328054 20 Modified 2017-10-23 revA by Dan/Matt
danstrider 10:085ab7328054 21 - linear actuator hardware works great, limit switches, sensing, etc.
danstrider 10:085ab7328054 22 - outer loops run, but move the BCE in the wrong direction.
danstrider 10:085ab7328054 23 - new IMU code doesn't read from the sensor correctly, but doesn't hang up either.
danstrider 10:085ab7328054 24 - depth sensor worked fine, just needs zero bias adjustment.
danstrider 10:085ab7328054 25 */
danstrider 10:085ab7328054 26
tzyoung 0:ea293bbf9717 27 #include "mbed.h"
tzyoung 0:ea293bbf9717 28 #include "StaticDefs.hpp"
tzyoung 2:892b58e56712 29 #include "config_functions.h"
tzyoung 2:892b58e56712 30
danstrider 10:085ab7328054 31 extern "C" void mbed_reset(); // utilized to reset the mbed
tzyoung 4:66f13fbb035d 32
danstrider 10:085ab7328054 33 void setup() {
danstrider 10:085ab7328054 34 pc().printf("\n\n\r\rFSG 2017-10-22 revD\n\r");
tzyoung 4:66f13fbb035d 35
danstrider 10:085ab7328054 36 // start up the system timer
tzyoung 0:ea293bbf9717 37 systemTime().start();
tzyoung 0:ea293bbf9717 38
danstrider 10:085ab7328054 39 // set up and start the adc. This runs on a fixed interval and is interrupt driven
tzyoung 0:ea293bbf9717 40 adc().initialize();
tzyoung 0:ea293bbf9717 41 adc().start();
danstrider 10:085ab7328054 42
danstrider 10:085ab7328054 43 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 44 imu().initialize();
danstrider 10:085ab7328054 45 imu().start();
danstrider 10:085ab7328054 46
danstrider 10:085ab7328054 47 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 10:085ab7328054 48 depth().initialize();
danstrider 10:085ab7328054 49
danstrider 10:085ab7328054 50 // construct a local file system
danstrider 10:085ab7328054 51 local();
tzyoung 0:ea293bbf9717 52
danstrider 10:085ab7328054 53 // load config data from files
danstrider 10:085ab7328054 54 load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
danstrider 10:085ab7328054 55 load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
danstrider 10:085ab7328054 56 load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt"
danstrider 10:085ab7328054 57 load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt"
tzyoung 4:66f13fbb035d 58
danstrider 10:085ab7328054 59 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 60 bce().init();
danstrider 10:085ab7328054 61 bce().start();
danstrider 10:085ab7328054 62 bce().setPosition_mm(bce().getPosition_mm()); // start by not moving
tzyoung 4:66f13fbb035d 63
danstrider 10:085ab7328054 64 batt().init();
tzyoung 2:892b58e56712 65 batt().start();
danstrider 10:085ab7328054 66 batt().setPosition_mm(batt().getPosition_mm()); // start by not moving
danstrider 10:085ab7328054 67
danstrider 10:085ab7328054 68 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 69 depthLoop().init();
danstrider 10:085ab7328054 70 depthLoop().setCommand(0.0);
mkelly10 5:15bd96205bb2 71
danstrider 10:085ab7328054 72 pitchLoop().init();
danstrider 10:085ab7328054 73 pitchLoop().setCommand(0.0);
danstrider 10:085ab7328054 74
danstrider 10:085ab7328054 75 // do not leave this in. Check that PID gains are loading
danstrider 10:085ab7328054 76 pc().printf("bce P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
danstrider 10:085ab7328054 77 pc().printf("batt P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
danstrider 10:085ab7328054 78 pc().printf("depth P: %3.2f, I: %3.2f, D %3.2f, \n\r", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD());
danstrider 10:085ab7328054 79 pc().printf("pitch P: %3.2f, I: %3.2f, D %3.2f, \n\r", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD());
danstrider 10:085ab7328054 80 pc().printf("\n\r");
danstrider 10:085ab7328054 81 }
mkelly10 5:15bd96205bb2 82
danstrider 10:085ab7328054 83 void keyboard() {
danstrider 10:085ab7328054 84 static float setpoint = 50.0;
danstrider 10:085ab7328054 85 static bool isOuterLoopActive = false; // allow user to select if the outer loop is active
danstrider 10:085ab7328054 86 char userInput;
danstrider 10:085ab7328054 87
danstrider 10:085ab7328054 88 // check keyboard and make settings changes as requested
danstrider 10:085ab7328054 89 if (pc().readable()) {
danstrider 10:085ab7328054 90 // get the key
danstrider 10:085ab7328054 91 userInput = pc().getc();
mkelly10 5:15bd96205bb2 92
danstrider 10:085ab7328054 93 // check command against desired control buttons
danstrider 10:085ab7328054 94 if (userInput == '+' or userInput == '=') {
danstrider 10:085ab7328054 95 setpoint += 1.0; //increment the command
danstrider 10:085ab7328054 96 }
danstrider 10:085ab7328054 97 else if (userInput == '-' or userInput == '_') {
danstrider 10:085ab7328054 98 setpoint -= 1.0; //decrement the command
danstrider 10:085ab7328054 99 }
danstrider 10:085ab7328054 100 else if (userInput == 't' or userInput == 'T') {
danstrider 10:085ab7328054 101 isOuterLoopActive = !isOuterLoopActive; // toggle the outer loop
danstrider 10:085ab7328054 102
danstrider 10:085ab7328054 103 if (isOuterLoopActive)
danstrider 10:085ab7328054 104 pc().printf("Outer loops are now active \n\r");
danstrider 10:085ab7328054 105 else
danstrider 10:085ab7328054 106 pc().printf("Outer loops are now inactive \n\r");
danstrider 10:085ab7328054 107 }
danstrider 10:085ab7328054 108 else if (userInput == '\r') {
danstrider 10:085ab7328054 109 if (isOuterLoopActive) {
danstrider 10:085ab7328054 110 pc().printf("setting outer loop: %3.0f \n\r", setpoint);
danstrider 10:085ab7328054 111
danstrider 10:085ab7328054 112 depthLoop().setCommand(setpoint);
danstrider 10:085ab7328054 113 bce().setPosition_mm(depthLoop().getOutput()); // connect outer to inner loop
danstrider 10:085ab7328054 114
danstrider 10:085ab7328054 115 pitchLoop().setCommand(setpoint);
danstrider 10:085ab7328054 116 batt().setPosition_mm(pitchLoop().getOutput()); // connect outer to inner loop
danstrider 10:085ab7328054 117 }
danstrider 10:085ab7328054 118 else {
danstrider 10:085ab7328054 119 pc().printf("setting inner loop: %3.0f \n\r", setpoint);
danstrider 10:085ab7328054 120
danstrider 10:085ab7328054 121 batt().setPosition_mm(setpoint);
danstrider 10:085ab7328054 122 bce().setPosition_mm(setpoint);
mkelly10 5:15bd96205bb2 123 }
mkelly10 5:15bd96205bb2 124 }
danstrider 10:085ab7328054 125 else if (userInput == '?') {
danstrider 10:085ab7328054 126 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
danstrider 10:085ab7328054 127 mbed_reset();
danstrider 10:085ab7328054 128 }
mkelly10 5:15bd96205bb2 129 }
danstrider 10:085ab7328054 130
danstrider 10:085ab7328054 131 if (isOuterLoopActive)
danstrider 10:085ab7328054 132 pc().printf("setpoint %3.1f, depth: %3.1f ft, bce: %3.1f mm \r", setpoint, depth().getDepth(), bce().getPosition_mm());
danstrider 10:085ab7328054 133 //pc().printf("setpoint %3.1f, pitch: %3.1f deg, batt: %3.1f mm \r", setpoint, imu().getPitch(), batt().getPosition_mm());
danstrider 10:085ab7328054 134 else
danstrider 10:085ab7328054 135 pc().printf("setpoint %3.1f, bce: %3.1f mm \r", setpoint, bce().getPosition_mm());
danstrider 10:085ab7328054 136 //pc().printf("setpoint %3.1f, batt: %3.1f mm \r", setpoint, batt().getPosition_mm());
mkelly10 5:15bd96205bb2 137 }
mkelly10 5:15bd96205bb2 138
danstrider 10:085ab7328054 139 void loop() {
danstrider 10:085ab7328054 140 led1() = !led1(); // blink
danstrider 10:085ab7328054 141
danstrider 10:085ab7328054 142 keyboard();
mkelly10 5:15bd96205bb2 143
danstrider 10:085ab7328054 144 // sequence controllers...
danstrider 10:085ab7328054 145 // check whether depth has been triggered
danstrider 10:085ab7328054 146 // if so, move on to the next line of the script
danstrider 10:085ab7328054 147 // if done, surface or repeat
danstrider 10:085ab7328054 148 }
tzyoung 0:ea293bbf9717 149
danstrider 10:085ab7328054 150 int main() {
danstrider 10:085ab7328054 151 setup();
danstrider 10:085ab7328054 152 while(1) {
danstrider 10:085ab7328054 153 loop();
danstrider 10:085ab7328054 154 }
danstrider 10:085ab7328054 155 }