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@48:20e681885161, 2018-02-15 (annotated)

Committer:
tnhnrl
Date:
Thu Feb 15 04:01:57 2018 +0000
Revision:
48:20e681885161
Parent:
47:fb3c7929d3f3
Child:
49:47ffa4feb6db
double-checking some data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
tnhnrl 48:20e681885161 2 Modified 2018-02-01 revA by Troy
tnhnrl 48:20e681885161 3 - Fixed bugs with file opening and closing
tnhnrl 48:20e681885161 4 - Made a directory printing function in the MbedLogger class that does not use the Directory List library (removed)
tnhnrl 48:20e681885161 5 Modified 2018-02-02 revA by Troy
tnhnrl 48:20e681885161 6 - Placed mbed file transmitter functionality into MbedLogger class
tnhnrl 48:20e681885161 7 - created a macro for printing out files to PC and serial port simultaneously (placed in main)
tnhnrl 48:20e681885161 8 Modified 2018-02-07 revA by Troy
tnhnrl 48:20e681885161 9 - SDFileSystem class added to code along with sd card file system in System "StaticDefs"
tnhnrl 48:20e681885161 10 - Had to fix bug with SDFileSystem, the imported version uses a buggy FATFileSystem class
tnhnrl 48:20e681885161 11 - Update came from https://os.mbed.com/teams/mbed-official/code/FATFileSystem/ (not the library importer tool)
tnhnrl 48:20e681885161 12 - Modified MbedLogger constructor to include string for file system, e.g. "/sd/" and "/local/"
tnhnrl 48:20e681885161 13 - Modified MbedLogger class to use local or sd card
tnhnrl 48:20e681885161 14 - Created SD card file system using reader on pins p11, p12, p13, p14 (SPI interface)
tnhnrl 48:20e681885161 15 Modified 2018-02-08 revA by Troy
tnhnrl 48:20e681885161 16 - Test code without ADC running to check behavior (timing issue)
tnhnrl 48:20e681885161 17 - Modified sequence to use sequence.txt and fixed warnings
tnhnrl 48:20e681885161 18 - Fixed CRC issue with file reception (the command that the MBED sends out)
tnhnrl 48:20e681885161 19 - Issue with IMU data on p27, need to check wiring and or MAX232
tnhnrl 48:20e681885161 20 Modified 2018-02-14 revA by Troy
tnhnrl 48:20e681885161 21 - IMU tested and working correctly
tnhnrl 48:20e681885161 22 - Data logging on SD card and MBED working simultaneously
tnhnrl 48:20e681885161 23 - State Machine is being used to transmit data over Xbee instead of a separate ticker
tnhnrl 48:20e681885161 24 - Servo driver tested separately in FSG_servo_test program (verified servo works separately)
tnhnrl 48:20e681885161 25 - Electrical issue with PWM signal interfering with motor driver signal (disabled for now)
tnhnrl 48:20e681885161 26 -
danstrider 10:085ab7328054 27 */
tnhnrl 20:8987a9ae2bc7 28
tzyoung 0:ea293bbf9717 29 #include "mbed.h"
tzyoung 0:ea293bbf9717 30 #include "StaticDefs.hpp"
tnhnrl 20:8987a9ae2bc7 31
tnhnrl 32:f2f8ae34aadc 32 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 20:8987a9ae2bc7 33 Ticker loop_rate_ticker;
tnhnrl 34:9b66c5188051 34 Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 35
tnhnrl 32:f2f8ae34aadc 36 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 37 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 38
tnhnrl 32:f2f8ae34aadc 39 void loop_trigger() { loop = true;} // loop trigger (used in while loop)
tnhnrl 34:9b66c5188051 40 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
tnhnrl 32:f2f8ae34aadc 41
danstrider 10:085ab7328054 42 void setup() {
danstrider 11:3b241ecb75ed 43 pc().baud(57600);
tnhnrl 39:58375ca6b6ff 44 pc().printf("\n\n\rFSG POOL TEST 2017-12-21 revA\n\n\r");
tnhnrl 32:f2f8ae34aadc 45
tnhnrl 32:f2f8ae34aadc 46 //setup data logger baud rate and write the start of the program (every time you reset)
tnhnrl 32:f2f8ae34aadc 47 datalogger().baud(57600);
tnhnrl 39:58375ca6b6ff 48 datalogger().printf("SYSTEM, RESET\n");
tnhnrl 20:8987a9ae2bc7 49
danstrider 10:085ab7328054 50 // start up the system timer
tzyoung 0:ea293bbf9717 51 systemTime().start();
tnhnrl 20:8987a9ae2bc7 52
danstrider 10:085ab7328054 53 // set up and start the adc. This runs on a fixed interval and is interrupt driven
tzyoung 0:ea293bbf9717 54 adc().initialize();
tzyoung 0:ea293bbf9717 55 adc().start();
danstrider 10:085ab7328054 56
danstrider 10:085ab7328054 57 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 58 imu().initialize();
danstrider 10:085ab7328054 59 imu().start();
danstrider 10:085ab7328054 60
danstrider 10:085ab7328054 61 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 14:85b64a4d08e8 62 depth().init();
danstrider 14:85b64a4d08e8 63 depth().tare();
danstrider 10:085ab7328054 64
tnhnrl 48:20e681885161 65 // construct the MBED local file system
danstrider 10:085ab7328054 66 local();
tnhnrl 48:20e681885161 67
tnhnrl 48:20e681885161 68 // construct the SD card file system
tnhnrl 48:20e681885161 69 sd_card();
tnhnrl 20:8987a9ae2bc7 70
danstrider 10:085ab7328054 71 // load config data from files
tnhnrl 21:38c8544db6f4 72 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 21:38c8544db6f4 73 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 21:38c8544db6f4 74 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 21:38c8544db6f4 75 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 20:8987a9ae2bc7 76
tnhnrl 43:891baf306e0a 77
danstrider 10:085ab7328054 78 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 79 bce().init();
danstrider 10:085ab7328054 80 bce().start();
mkelly10 12:a0519d11d2b6 81 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 82
danstrider 10:085ab7328054 83 batt().init();
tzyoung 2:892b58e56712 84 batt().start();
mkelly10 12:a0519d11d2b6 85 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 86
danstrider 10:085ab7328054 87 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 88 depthLoop().init();
tnhnrl 13:84fcbe1dcd62 89 depthLoop().start();
tnhnrl 16:3363b9f14913 90 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 91
danstrider 10:085ab7328054 92 pitchLoop().init();
tnhnrl 13:84fcbe1dcd62 93 pitchLoop().start();
tnhnrl 16:3363b9f14913 94 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 20:8987a9ae2bc7 95
tnhnrl 20:8987a9ae2bc7 96 // show that the PID gains are loading from the file
tnhnrl 38:83d06c294807 97 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 98 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 99 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 100 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 101 pc().printf("\n\r");
tnhnrl 21:38c8544db6f4 102
tnhnrl 17:7c16b5671d0e 103 //load sequence from file
tnhnrl 17:7c16b5671d0e 104 sequenceController().loadSequence();
tnhnrl 20:8987a9ae2bc7 105
tnhnrl 20:8987a9ae2bc7 106 // establish the main loop rate
tnhnrl 20:8987a9ae2bc7 107 loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
tnhnrl 32:f2f8ae34aadc 108
tnhnrl 32:f2f8ae34aadc 109 // setup the data logger rate
tnhnrl 34:9b66c5188051 110 log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
tnhnrl 32:f2f8ae34aadc 111
tnhnrl 35:2f66ea4863d5 112 //create the MBED log file (current log file)
tnhnrl 36:966a86937e17 113 //mbedLogger().openFile();
tnhnrl 45:16b8162188ca 114
tnhnrl 47:fb3c7929d3f3 115 //set time of logger (to current or close-to-current time)
tnhnrl 47:fb3c7929d3f3 116 mbedLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 117 sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 118
tnhnrl 47:fb3c7929d3f3 119 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 120 mbedLogger().initializeLogFile();
tnhnrl 48:20e681885161 121 sdLogger().initializeLogFile();
tnhnrl 47:fb3c7929d3f3 122
tnhnrl 45:16b8162188ca 123 pc().printf("Size of ConfigFile: %d\n\r", sizeof(ConfigFile));
tnhnrl 45:16b8162188ca 124 pc().printf("Size of ConfigFileIO: %d\n\r", sizeof(ConfigFileIO));
tnhnrl 45:16b8162188ca 125 pc().printf("Size of IMU: %d\n\r", sizeof(IMU));
tnhnrl 45:16b8162188ca 126 pc().printf("Size of LinearActuator: %d\n\r", sizeof(LinearActuator));
tnhnrl 45:16b8162188ca 127 pc().printf("Size of LTC1298 (SpiADC): %d\n\r", sizeof(SpiADC));
tnhnrl 45:16b8162188ca 128 pc().printf("Size of MbedLogger: %d\n\r", sizeof(MbedLogger));
tnhnrl 45:16b8162188ca 129 pc().printf("Size of omegaPX209: %d\n\r", sizeof(omegaPX209));
tnhnrl 45:16b8162188ca 130 pc().printf("Size of OuterLoop: %d\n\r", sizeof(OuterLoop));
tnhnrl 45:16b8162188ca 131 pc().printf("Size of PIDController: %d\n\r", sizeof(PIDController));
tnhnrl 45:16b8162188ca 132 pc().printf("Size of PololuHBridge: %d\n\r", sizeof(PololuHBridge)); //fix this class
tnhnrl 45:16b8162188ca 133 pc().printf("Size of PosVelFilter: %d\n\r", sizeof(PosVelFilter));
tnhnrl 45:16b8162188ca 134 pc().printf("Size of SequenceController: %d\n\r", sizeof(SequenceController));
tnhnrl 45:16b8162188ca 135 pc().printf("Size of ServoDriver: %d\n\r", sizeof(ServoDriver));
tnhnrl 45:16b8162188ca 136 pc().printf("Size of StateMachine: %d\n\r", sizeof(StateMachine));
tnhnrl 21:38c8544db6f4 137 }
tnhnrl 20:8987a9ae2bc7 138
danstrider 10:085ab7328054 139 int main() {
danstrider 10:085ab7328054 140 setup();
tnhnrl 16:3363b9f14913 141
danstrider 10:085ab7328054 142 while(1) {
tnhnrl 34:9b66c5188051 143 static int current_state = 0;
tnhnrl 36:966a86937e17 144
tnhnrl 36:966a86937e17 145 static bool file_opened = false;
tnhnrl 36:966a86937e17 146
tnhnrl 39:58375ca6b6ff 147 // FSM loop runs at 10 hz
tnhnrl 20:8987a9ae2bc7 148 if(loop) {
tnhnrl 32:f2f8ae34aadc 149 led1() = !led1(); // blink led 1
tnhnrl 45:16b8162188ca 150 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 20:8987a9ae2bc7 151 loop = false; // wait until the loop rate timer fires again
tnhnrl 17:7c16b5671d0e 152 }
tnhnrl 32:f2f8ae34aadc 153
tnhnrl 39:58375ca6b6ff 154 // log loop runs at 1 hz
tnhnrl 34:9b66c5188051 155 if (log_loop) {
tnhnrl 45:16b8162188ca 156 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 45:16b8162188ca 157 if (current_state == TRANSMIT_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 45:16b8162188ca 158 //main_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 159 //log_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 160
tnhnrl 45:16b8162188ca 161 ; //pass
tnhnrl 45:16b8162188ca 162 }
tnhnrl 34:9b66c5188051 163
tnhnrl 45:16b8162188ca 164 else if(current_state != 0) {
tnhnrl 45:16b8162188ca 165 if (!file_opened) { //if the log file is not open, open it
tnhnrl 45:16b8162188ca 166 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 48:20e681885161 167 sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 45:16b8162188ca 168
tnhnrl 45:16b8162188ca 169 file_opened = true; //stops it from continuing to open it
tnhnrl 37:357e98a929cc 170
tnhnrl 36:966a86937e17 171 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 172 }
tnhnrl 36:966a86937e17 173
tnhnrl 45:16b8162188ca 174 //record to Mbed file system
tnhnrl 45:16b8162188ca 175 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 48:20e681885161 176 sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 36:966a86937e17 177 }
tnhnrl 36:966a86937e17 178
tnhnrl 39:58375ca6b6ff 179 //when the current FSM state is zero, reset the file
tnhnrl 36:966a86937e17 180 else {
tnhnrl 36:966a86937e17 181 //this can only happen once
tnhnrl 36:966a86937e17 182 if (file_opened) {
tnhnrl 45:16b8162188ca 183 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 48:20e681885161 184 sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 45:16b8162188ca 185
tnhnrl 36:966a86937e17 186 file_opened = false;
tnhnrl 37:357e98a929cc 187
tnhnrl 37:357e98a929cc 188 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 189 }
tnhnrl 34:9b66c5188051 190 }
tnhnrl 34:9b66c5188051 191
tnhnrl 34:9b66c5188051 192 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 45:16b8162188ca 193 } //END OF LOG LOOP
danstrider 10:085ab7328054 194 }
danstrider 10:085ab7328054 195 }