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

Wed Feb 14 21:54:03 2018 +0000
added new configfileIO

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 21:38c8544db6f4 99 }
tnhnrl 20:8987a9ae2bc7 100
danstrider 10:085ab7328054 101 int main() {
danstrider 10:085ab7328054 102 setup();
tnhnrl 16:3363b9f14913 103
danstrider 10:085ab7328054 104 while(1) {
tnhnrl 34:9b66c5188051 105 static int current_state = 0;
tnhnrl 36:966a86937e17 106
tnhnrl 36:966a86937e17 107 static bool file_opened = false;
tnhnrl 36:966a86937e17 108
tnhnrl 39:58375ca6b6ff 109 // FSM loop runs at 10 hz
tnhnrl 20:8987a9ae2bc7 110 if(loop) {
tnhnrl 32:f2f8ae34aadc 111 led1() = !led1(); // blink led 1
tnhnrl 34:9b66c5188051 112
tnhnrl 39:58375ca6b6ff 113 //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 34:9b66c5188051 114 current_state = stateMachine().runStateMachine();
tnhnrl 34:9b66c5188051 115
tnhnrl 20:8987a9ae2bc7 116 loop = false; // wait until the loop rate timer fires again
tnhnrl 17:7c16b5671d0e 117 }
tnhnrl 32:f2f8ae34aadc 118
tnhnrl 39:58375ca6b6ff 119 // log loop runs at 1 hz
tnhnrl 34:9b66c5188051 120 if (log_loop) {
tnhnrl 34:9b66c5188051 121 led3() = !led3(); // blink led 3
tnhnrl 34:9b66c5188051 122
tnhnrl 37:357e98a929cc 123 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 34:9b66c5188051 124 if(current_state != 0) {
tnhnrl 36:966a86937e17 125 //pc().printf("\n\rDEBUG: trying to record data \n\r");
tnhnrl 36:966a86937e17 126
tnhnrl 37:357e98a929cc 127 //if the log file is not open, open it
tnhnrl 36:966a86937e17 128 if (!file_opened) {
tnhnrl 36:966a86937e17 129 mbedLogger().openFile(); //open MBED file once
tnhnrl 36:966a86937e17 130 file_opened = true; //stops it from continuing to open it
tnhnrl 37:357e98a929cc 131
tnhnrl 36:966a86937e17 132 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 133 }
tnhnrl 36:966a86937e17 134
tnhnrl 36:966a86937e17 135 //record to OpenLog hardware
tnhnrl 36:966a86937e17 136 OpenLog().recordData(current_state); //start recording
tnhnrl 36:966a86937e17 137
tnhnrl 39:58375ca6b6ff 138 //record to Mbed file system
tnhnrl 36:966a86937e17 139 mbedLogger().saveDataToFile(current_state,stateMachine().dataArray());
tnhnrl 36:966a86937e17 140 }
tnhnrl 36:966a86937e17 141
tnhnrl 39:58375ca6b6ff 142 //when the current FSM state is zero, reset the file
tnhnrl 36:966a86937e17 143 else {
tnhnrl 36:966a86937e17 144 //this can only happen once
tnhnrl 36:966a86937e17 145 if (file_opened) {
tnhnrl 36:966a86937e17 146 mbedLogger().closeFile();
tnhnrl 36:966a86937e17 147 file_opened = false;
tnhnrl 37:357e98a929cc 148
tnhnrl 37:357e98a929cc 149 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 150 }
tnhnrl 34:9b66c5188051 151 }
tnhnrl 34:9b66c5188051 152
tnhnrl 34:9b66c5188051 153 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 34:9b66c5188051 154 }
danstrider 10:085ab7328054 155 }
danstrider 10:085ab7328054 156 }