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@47:fb3c7929d3f3, 2018-02-15 (annotated)

Committer:
tnhnrl
Date:
Thu Feb 15 03:07:16 2018 +0000
Revision:
47:fb3c7929d3f3
Parent:
45:16b8162188ca
Child:
48:20e681885161
everything except sd card loader...what's up

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
tnhnrl 32:f2f8ae34aadc 2 Modified 2017-12-19 revA by Troy
tnhnrl 32:f2f8ae34aadc 3 - Fixed OpenLog printing to include states and variable names. Currently logs to LOG#####.TXT files
tnhnrl 32:f2f8ae34aadc 4 1) Note: The OpenLog only starts a new log when it is power-cycled (with the MBED)
tnhnrl 34:9b66c5188051 5 Modified 2017-12-20 revA by Troy
tnhnrl 32:f2f8ae34aadc 6 - Modified code to log every 1 second in current iteration
tnhnrl 34:9b66c5188051 7 Modified 2017-12-20 revB by Troy
tnhnrl 34:9b66c5188051 8 - Fixed bug where Dive depth was resetting to rise depth
tnhnrl 37:357e98a929cc 9 Modified 2017-12-21 revA by Troy
tnhnrl 37:357e98a929cc 10 - 2 minute timeout default
tnhnrl 39:58375ca6b6ff 11 - Added system time to MBED logger and OpenLog
tnhnrl 37:357e98a929cc 12 - Fixed bug where it was recording random keyboard presses (other FSM states)
tnhnrl 38:83d06c294807 13 - Added the ability to save the batt and BCE PID config files
tnhnrl 39:58375ca6b6ff 14 - Fixed bug with FIND_NEUTRAL sub-FSM (command was not updating with each timer cycle)
danstrider 10:085ab7328054 15 */
tnhnrl 20:8987a9ae2bc7 16
tzyoung 0:ea293bbf9717 17 #include "mbed.h"
tzyoung 0:ea293bbf9717 18 #include "StaticDefs.hpp"
tnhnrl 20:8987a9ae2bc7 19
tnhnrl 32:f2f8ae34aadc 20 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 20:8987a9ae2bc7 21 Ticker loop_rate_ticker;
tnhnrl 34:9b66c5188051 22 Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 23
tnhnrl 32:f2f8ae34aadc 24 volatile bool loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
tnhnrl 34:9b66c5188051 25 volatile bool log_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
tnhnrl 32:f2f8ae34aadc 26
tnhnrl 32:f2f8ae34aadc 27 void loop_trigger() { loop = true;} // loop trigger (used in while loop)
tnhnrl 34:9b66c5188051 28 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
tnhnrl 32:f2f8ae34aadc 29
danstrider 10:085ab7328054 30 void setup() {
danstrider 11:3b241ecb75ed 31 pc().baud(57600);
tnhnrl 39:58375ca6b6ff 32 pc().printf("\n\n\rFSG POOL TEST 2017-12-21 revA\n\n\r");
tnhnrl 32:f2f8ae34aadc 33
tnhnrl 32:f2f8ae34aadc 34 //setup data logger baud rate and write the start of the program (every time you reset)
tnhnrl 32:f2f8ae34aadc 35 datalogger().baud(57600);
tnhnrl 39:58375ca6b6ff 36 datalogger().printf("SYSTEM, RESET\n");
tnhnrl 20:8987a9ae2bc7 37
danstrider 10:085ab7328054 38 // start up the system timer
tzyoung 0:ea293bbf9717 39 systemTime().start();
tnhnrl 20:8987a9ae2bc7 40
danstrider 10:085ab7328054 41 // set up and start the adc. This runs on a fixed interval and is interrupt driven
tzyoung 0:ea293bbf9717 42 adc().initialize();
tzyoung 0:ea293bbf9717 43 adc().start();
danstrider 10:085ab7328054 44
danstrider 10:085ab7328054 45 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 46 imu().initialize();
danstrider 10:085ab7328054 47 imu().start();
danstrider 10:085ab7328054 48
danstrider 10:085ab7328054 49 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 14:85b64a4d08e8 50 depth().init();
danstrider 14:85b64a4d08e8 51 depth().tare();
danstrider 10:085ab7328054 52
danstrider 10:085ab7328054 53 // construct a local file system
danstrider 10:085ab7328054 54 local();
tnhnrl 20:8987a9ae2bc7 55
danstrider 10:085ab7328054 56 // load config data from files
tnhnrl 21:38c8544db6f4 57 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 21:38c8544db6f4 58 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 21:38c8544db6f4 59 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 21:38c8544db6f4 60 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 20:8987a9ae2bc7 61
tnhnrl 43:891baf306e0a 62
danstrider 10:085ab7328054 63 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 64 bce().init();
danstrider 10:085ab7328054 65 bce().start();
mkelly10 12:a0519d11d2b6 66 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 67
danstrider 10:085ab7328054 68 batt().init();
tzyoung 2:892b58e56712 69 batt().start();
mkelly10 12:a0519d11d2b6 70 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 71
danstrider 10:085ab7328054 72 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 73 depthLoop().init();
tnhnrl 13:84fcbe1dcd62 74 depthLoop().start();
tnhnrl 16:3363b9f14913 75 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 76
danstrider 10:085ab7328054 77 pitchLoop().init();
tnhnrl 13:84fcbe1dcd62 78 pitchLoop().start();
tnhnrl 16:3363b9f14913 79 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 20:8987a9ae2bc7 80
tnhnrl 20:8987a9ae2bc7 81 // show that the PID gains are loading from the file
tnhnrl 38:83d06c294807 82 pc().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
tnhnrl 38:83d06c294807 83 pc().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
tnhnrl 21:38c8544db6f4 84 pc().printf("depth P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
tnhnrl 21:38c8544db6f4 85 pc().printf("pitch P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
danstrider 10:085ab7328054 86 pc().printf("\n\r");
tnhnrl 21:38c8544db6f4 87
tnhnrl 17:7c16b5671d0e 88 //load sequence from file
tnhnrl 17:7c16b5671d0e 89 sequenceController().loadSequence();
tnhnrl 20:8987a9ae2bc7 90
tnhnrl 20:8987a9ae2bc7 91 // establish the main loop rate
tnhnrl 20:8987a9ae2bc7 92 loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
tnhnrl 32:f2f8ae34aadc 93
tnhnrl 32:f2f8ae34aadc 94 // setup the data logger rate
tnhnrl 34:9b66c5188051 95 log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
tnhnrl 32:f2f8ae34aadc 96
tnhnrl 35:2f66ea4863d5 97 //create the MBED log file (current log file)
tnhnrl 36:966a86937e17 98 //mbedLogger().openFile();
tnhnrl 45:16b8162188ca 99
tnhnrl 47:fb3c7929d3f3 100 //set time of logger (to current or close-to-current time)
tnhnrl 47:fb3c7929d3f3 101 mbedLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 102 sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 103
tnhnrl 47:fb3c7929d3f3 104 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 105 mbedLogger().initializeLogFile();
tnhnrl 47:fb3c7929d3f3 106
tnhnrl 45:16b8162188ca 107 pc().printf("Size of ConfigFile: %d\n\r", sizeof(ConfigFile));
tnhnrl 45:16b8162188ca 108 pc().printf("Size of ConfigFileIO: %d\n\r", sizeof(ConfigFileIO));
tnhnrl 45:16b8162188ca 109 pc().printf("Size of IMU: %d\n\r", sizeof(IMU));
tnhnrl 45:16b8162188ca 110 pc().printf("Size of LinearActuator: %d\n\r", sizeof(LinearActuator));
tnhnrl 45:16b8162188ca 111 pc().printf("Size of LTC1298 (SpiADC): %d\n\r", sizeof(SpiADC));
tnhnrl 45:16b8162188ca 112 pc().printf("Size of MbedLogger: %d\n\r", sizeof(MbedLogger));
tnhnrl 45:16b8162188ca 113 pc().printf("Size of omegaPX209: %d\n\r", sizeof(omegaPX209));
tnhnrl 45:16b8162188ca 114 pc().printf("Size of OuterLoop: %d\n\r", sizeof(OuterLoop));
tnhnrl 45:16b8162188ca 115 pc().printf("Size of PIDController: %d\n\r", sizeof(PIDController));
tnhnrl 45:16b8162188ca 116 pc().printf("Size of PololuHBridge: %d\n\r", sizeof(PololuHBridge)); //fix this class
tnhnrl 45:16b8162188ca 117 pc().printf("Size of PosVelFilter: %d\n\r", sizeof(PosVelFilter));
tnhnrl 45:16b8162188ca 118 pc().printf("Size of SequenceController: %d\n\r", sizeof(SequenceController));
tnhnrl 45:16b8162188ca 119 pc().printf("Size of ServoDriver: %d\n\r", sizeof(ServoDriver));
tnhnrl 45:16b8162188ca 120 pc().printf("Size of StateMachine: %d\n\r", sizeof(StateMachine));
tnhnrl 21:38c8544db6f4 121 }
tnhnrl 20:8987a9ae2bc7 122
danstrider 10:085ab7328054 123 int main() {
danstrider 10:085ab7328054 124 setup();
tnhnrl 16:3363b9f14913 125
danstrider 10:085ab7328054 126 while(1) {
tnhnrl 34:9b66c5188051 127 static int current_state = 0;
tnhnrl 36:966a86937e17 128
tnhnrl 36:966a86937e17 129 static bool file_opened = false;
tnhnrl 36:966a86937e17 130
tnhnrl 39:58375ca6b6ff 131 // FSM loop runs at 10 hz
tnhnrl 20:8987a9ae2bc7 132 if(loop) {
tnhnrl 32:f2f8ae34aadc 133 led1() = !led1(); // blink led 1
tnhnrl 45:16b8162188ca 134 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 20:8987a9ae2bc7 135 loop = false; // wait until the loop rate timer fires again
tnhnrl 17:7c16b5671d0e 136 }
tnhnrl 32:f2f8ae34aadc 137
tnhnrl 39:58375ca6b6ff 138 // log loop runs at 1 hz
tnhnrl 34:9b66c5188051 139 if (log_loop) {
tnhnrl 45:16b8162188ca 140 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 45:16b8162188ca 141 if (current_state == TRANSMIT_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 45:16b8162188ca 142 //main_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 143 //log_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 144
tnhnrl 45:16b8162188ca 145 ; //pass
tnhnrl 45:16b8162188ca 146 }
tnhnrl 34:9b66c5188051 147
tnhnrl 45:16b8162188ca 148 else if(current_state != 0) {
tnhnrl 45:16b8162188ca 149 if (!file_opened) { //if the log file is not open, open it
tnhnrl 45:16b8162188ca 150 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 45:16b8162188ca 151 //sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 45:16b8162188ca 152
tnhnrl 45:16b8162188ca 153 file_opened = true; //stops it from continuing to open it
tnhnrl 37:357e98a929cc 154
tnhnrl 36:966a86937e17 155 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 156 }
tnhnrl 36:966a86937e17 157
tnhnrl 45:16b8162188ca 158 //record to Mbed file system
tnhnrl 45:16b8162188ca 159 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 45:16b8162188ca 160 //sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 36:966a86937e17 161 }
tnhnrl 36:966a86937e17 162
tnhnrl 39:58375ca6b6ff 163 //when the current FSM state is zero, reset the file
tnhnrl 36:966a86937e17 164 else {
tnhnrl 36:966a86937e17 165 //this can only happen once
tnhnrl 36:966a86937e17 166 if (file_opened) {
tnhnrl 45:16b8162188ca 167 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 45:16b8162188ca 168 //sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 45:16b8162188ca 169
tnhnrl 36:966a86937e17 170 file_opened = false;
tnhnrl 37:357e98a929cc 171
tnhnrl 37:357e98a929cc 172 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 173 }
tnhnrl 34:9b66c5188051 174 }
tnhnrl 34:9b66c5188051 175
tnhnrl 34:9b66c5188051 176 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 45:16b8162188ca 177 } //END OF LOG LOOP
danstrider 10:085ab7328054 178 }
danstrider 10:085ab7328054 179 }