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@52:f207567d3ea4, 2018-06-06 (annotated)

Committer:
tnhnrl
Date:
Wed Jun 06 19:18:47 2018 +0000
Revision:
52:f207567d3ea4
Parent:
51:c5c40272ecc3
Child:
53:c0586fe62b01
version with non-working motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
tnhnrl 52:f207567d3ea4 2 Modified FSG PCB V1_5
tnhnrl 52:f207567d3ea4 3 - Freezes when doing a dive or any timed sequence (commented out SD card references)
tnhnrl 52:f207567d3ea4 4 - commented out sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 52:f207567d3ea4 5 - commented out sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 52:f207567d3ea4 6 - commented out sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 52:f207567d3ea4 7 - reduced timer to 20 seconds for bench testing
tnhnrl 52:f207567d3ea4 8 - modified ConfigFileIO for rudder()
tnhnrl 52:f207567d3ea4 9 - added in getFloatUserInput function from newer code
danstrider 10:085ab7328054 10 */
tnhnrl 20:8987a9ae2bc7 11
tzyoung 0:ea293bbf9717 12 #include "mbed.h"
tzyoung 0:ea293bbf9717 13 #include "StaticDefs.hpp"
tnhnrl 20:8987a9ae2bc7 14
tnhnrl 32:f2f8ae34aadc 15 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 20:8987a9ae2bc7 16 Ticker loop_rate_ticker;
tnhnrl 34:9b66c5188051 17 Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 18
tnhnrl 32:f2f8ae34aadc 19 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 20 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 21
tnhnrl 32:f2f8ae34aadc 22 void loop_trigger() { loop = true;} // loop trigger (used in while loop)
tnhnrl 34:9b66c5188051 23 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
tnhnrl 32:f2f8ae34aadc 24
danstrider 10:085ab7328054 25 void setup() {
danstrider 11:3b241ecb75ed 26 pc().baud(57600);
tnhnrl 52:f207567d3ea4 27 pc().printf("\n\n\rFSG PCB V1.5 2018-06-06 \n\n\r");
mkelly10 51:c5c40272ecc3 28
mkelly10 51:c5c40272ecc3 29 /* //setup data logger baud rate and write the start of the program (every time you reset)
tnhnrl 32:f2f8ae34aadc 30 datalogger().baud(57600);
tnhnrl 39:58375ca6b6ff 31 datalogger().printf("SYSTEM, RESET\n");
mkelly10 51:c5c40272ecc3 32 */
mkelly10 51:c5c40272ecc3 33
danstrider 10:085ab7328054 34 // start up the system timer
tzyoung 0:ea293bbf9717 35 systemTime().start();
tnhnrl 20:8987a9ae2bc7 36
danstrider 10:085ab7328054 37 // set up and start the adc. This runs on a fixed interval and is interrupt driven
mkelly10 51:c5c40272ecc3 38 adc().initialize();
tzyoung 0:ea293bbf9717 39 adc().start();
danstrider 10:085ab7328054 40
danstrider 10:085ab7328054 41 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 42 imu().initialize();
danstrider 10:085ab7328054 43 imu().start();
danstrider 10:085ab7328054 44
danstrider 10:085ab7328054 45 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 14:85b64a4d08e8 46 depth().init();
danstrider 14:85b64a4d08e8 47 depth().tare();
danstrider 10:085ab7328054 48
tnhnrl 48:20e681885161 49 // construct the MBED local file system
danstrider 10:085ab7328054 50 local();
tnhnrl 48:20e681885161 51
tnhnrl 48:20e681885161 52 // construct the SD card file system
mkelly10 51:c5c40272ecc3 53 // sd_card();
tnhnrl 20:8987a9ae2bc7 54
danstrider 10:085ab7328054 55 // load config data from files
tnhnrl 21:38c8544db6f4 56 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 21:38c8544db6f4 57 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 21:38c8544db6f4 58 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 21:38c8544db6f4 59 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 20:8987a9ae2bc7 60
tnhnrl 43:891baf306e0a 61
danstrider 10:085ab7328054 62 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 63 bce().init();
danstrider 10:085ab7328054 64 bce().start();
mkelly10 12:a0519d11d2b6 65 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 66
danstrider 10:085ab7328054 67 batt().init();
tzyoung 2:892b58e56712 68 batt().start();
mkelly10 12:a0519d11d2b6 69 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 70
danstrider 10:085ab7328054 71 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 72 depthLoop().init();
tnhnrl 13:84fcbe1dcd62 73 depthLoop().start();
tnhnrl 16:3363b9f14913 74 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 75
danstrider 10:085ab7328054 76 pitchLoop().init();
tnhnrl 13:84fcbe1dcd62 77 pitchLoop().start();
tnhnrl 16:3363b9f14913 78 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 20:8987a9ae2bc7 79
tnhnrl 20:8987a9ae2bc7 80 // show that the PID gains are loading from the file
tnhnrl 38:83d06c294807 81 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 82 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 83 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 84 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 85 pc().printf("\n\r");
tnhnrl 21:38c8544db6f4 86
tnhnrl 17:7c16b5671d0e 87 //load sequence from file
tnhnrl 17:7c16b5671d0e 88 sequenceController().loadSequence();
tnhnrl 20:8987a9ae2bc7 89
tnhnrl 20:8987a9ae2bc7 90 // establish the main loop rate
tnhnrl 20:8987a9ae2bc7 91 loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
tnhnrl 32:f2f8ae34aadc 92
tnhnrl 32:f2f8ae34aadc 93 // setup the data logger rate
tnhnrl 34:9b66c5188051 94 log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
mkelly10 51:c5c40272ecc3 95
tnhnrl 47:fb3c7929d3f3 96 //set time of logger (to current or close-to-current time)
mkelly10 51:c5c40272ecc3 97 mbedLogger().setLogTime();
mkelly10 51:c5c40272ecc3 98 // sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 99
tnhnrl 47:fb3c7929d3f3 100 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 101 mbedLogger().initializeLogFile();
mkelly10 51:c5c40272ecc3 102 // sdLogger().initializeLogFile();
mkelly10 51:c5c40272ecc3 103
tnhnrl 21:38c8544db6f4 104 }
tnhnrl 20:8987a9ae2bc7 105
danstrider 10:085ab7328054 106 int main() {
danstrider 10:085ab7328054 107 setup();
mkelly10 51:c5c40272ecc3 108
danstrider 10:085ab7328054 109 while(1) {
tnhnrl 34:9b66c5188051 110 static int current_state = 0;
tnhnrl 36:966a86937e17 111
tnhnrl 36:966a86937e17 112 static bool file_opened = false;
tnhnrl 36:966a86937e17 113
tnhnrl 39:58375ca6b6ff 114 // FSM loop runs at 10 hz
tnhnrl 20:8987a9ae2bc7 115 if(loop) {
tnhnrl 32:f2f8ae34aadc 116 led1() = !led1(); // blink led 1
tnhnrl 45:16b8162188ca 117 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 20:8987a9ae2bc7 118 loop = false; // wait until the loop rate timer fires again
tnhnrl 17:7c16b5671d0e 119 }
tnhnrl 32:f2f8ae34aadc 120
tnhnrl 39:58375ca6b6ff 121 // log loop runs at 1 hz
tnhnrl 34:9b66c5188051 122 if (log_loop) {
tnhnrl 45:16b8162188ca 123 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 45:16b8162188ca 124 if (current_state == TRANSMIT_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 45:16b8162188ca 125 //main_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 126 //log_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 127
tnhnrl 45:16b8162188ca 128 ; //pass
tnhnrl 45:16b8162188ca 129 }
tnhnrl 34:9b66c5188051 130
tnhnrl 45:16b8162188ca 131 else if(current_state != 0) {
tnhnrl 45:16b8162188ca 132 if (!file_opened) { //if the log file is not open, open it
tnhnrl 45:16b8162188ca 133 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 52:f207567d3ea4 134 // sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 45:16b8162188ca 135
tnhnrl 45:16b8162188ca 136 file_opened = true; //stops it from continuing to open it
tnhnrl 37:357e98a929cc 137
tnhnrl 36:966a86937e17 138 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 139 }
tnhnrl 36:966a86937e17 140
tnhnrl 45:16b8162188ca 141 //record to Mbed file system
tnhnrl 45:16b8162188ca 142 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 52:f207567d3ea4 143 // sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 36:966a86937e17 144 }
tnhnrl 36:966a86937e17 145
tnhnrl 39:58375ca6b6ff 146 //when the current FSM state is zero, reset the file
tnhnrl 36:966a86937e17 147 else {
tnhnrl 36:966a86937e17 148 //this can only happen once
tnhnrl 36:966a86937e17 149 if (file_opened) {
tnhnrl 45:16b8162188ca 150 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 52:f207567d3ea4 151 // sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 45:16b8162188ca 152
tnhnrl 36:966a86937e17 153 file_opened = false;
tnhnrl 37:357e98a929cc 154
tnhnrl 37:357e98a929cc 155 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 156 }
tnhnrl 34:9b66c5188051 157 }
tnhnrl 34:9b66c5188051 158
tnhnrl 34:9b66c5188051 159 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 45:16b8162188ca 160 } //END OF LOG LOOP
danstrider 10:085ab7328054 161 }
mkelly10 51:c5c40272ecc3 162
danstrider 10:085ab7328054 163 }