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@63:6cb0405fc6e6, 2018-06-18 (annotated)

Committer:
tnhnrl
Date:
Mon Jun 18 21:02:55 2018 +0000
Revision:
63:6cb0405fc6e6
Parent:
61:f7437daae608
Child:
65:2ac186553959
Version with code updates on multi-dive (still testing) and PID inputs and fixed data transmissions so it's working very quickly now on USB.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
tnhnrl 58:94b7fd55185e 2 Modified FSG PCB V_1_1
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
tnhnrl 53:c0586fe62b01 10 - changed LinearActuator & batt() in StaticDefs to match new pinouts from Bob/Matt/Troy fixes
tnhnrl 53:c0586fe62b01 11 - slowed down battery motor because it's silly fast at 30 volts (bench test)
tnhnrl 53:c0586fe62b01 12 * BCE gain is proportional 0.1 now 0.01
tnhnrl 53:c0586fe62b01 13 - BATT was moving in the wrong direction, gain was P: 0.10, I: 0.0, D: 0.0
tnhnrl 57:ec69651c8c21 14 * change gain to P: -0.10 (gain was flipped, I think the old circuit board had the voltages flipped ? ?)
tnhnrl 53:c0586fe62b01 15 - StateMachine changes for testing
tnhnrl 53:c0586fe62b01 16 * added keyboard_menu_STREAM_STATUS();
tnhnrl 53:c0586fe62b01 17 * added keyboard_menu_RUDDER_SERVO_settings();
tnhnrl 54:d4990fb68404 18 - modified the zero on the battery position from 610 to 836
tnhnrl 54:d4990fb68404 19 - BMM (batt) slope may be incorrect, seems low, currently 0.12176
tnhnrl 54:d4990fb68404 20 - modified the zero on BCE from 253 to 460
tnhnrl 54:d4990fb68404 21 - Pressure readings are wrong
tnhnrl 54:d4990fb68404 22 * added readADCCounts() to omegaPX209 class to see channel readings
tnhnrl 54:d4990fb68404 23 * modified omegaPX209 class to use filtered ADC readings from SpiADC.readCh4()
tnhnrl 54:d4990fb68404 24 - fixed rudderLoop to headingLoop from newer code
tnhnrl 58:94b7fd55185e 25 Modified FSG PCB V_1_2
tnhnrl 55:f4ec445c42fe 26 - added init headingLoop to main
tnhnrl 55:f4ec445c42fe 27 - added pitch and heading outputs to STREAM_STATUS
tnhnrl 54:d4990fb68404 28
tnhnrl 54:d4990fb68404 29 NOTE: Flipped motor controller output on connector side with battery mass mover (BMM)
tnhnrl 54:d4990fb68404 30 - Motor direction was opposite the BCE motor (because of gearing)
tnhnrl 54:d4990fb68404 31 - BMM P gain is now positive 0.02 (from -0.10)
tnhnrl 54:d4990fb68404 32
tnhnrl 58:94b7fd55185e 33 Modified FSG PCB V_1_3
tnhnrl 56:48a8a5a65b82 34 - added timing code for interrupt that drives the rudder (testing with o-scope)
tnhnrl 58:94b7fd55185e 35 - PID controller replaced with newer version from 5/29 code branch
tnhnrl 58:94b7fd55185e 36 - StateMachine hanged style of variables to match convention in code
tnhnrl 63:6cb0405fc6e6 37
tnhnrl 63:6cb0405fc6e6 38 Modified FSG PCB V_1_4
tnhnrl 63:6cb0405fc6e6 39 - adcLed = 0; in adc
danstrider 10:085ab7328054 40 */
tnhnrl 20:8987a9ae2bc7 41
tzyoung 0:ea293bbf9717 42 #include "mbed.h"
tzyoung 0:ea293bbf9717 43 #include "StaticDefs.hpp"
tnhnrl 20:8987a9ae2bc7 44
tnhnrl 32:f2f8ae34aadc 45 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 20:8987a9ae2bc7 46 Ticker loop_rate_ticker;
tnhnrl 34:9b66c5188051 47 Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 48
tnhnrl 32:f2f8ae34aadc 49 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 50 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 51
tnhnrl 32:f2f8ae34aadc 52 void loop_trigger() { loop = true;} // loop trigger (used in while loop)
tnhnrl 34:9b66c5188051 53 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
tnhnrl 32:f2f8ae34aadc 54
tnhnrl 56:48a8a5a65b82 55 /****************** for rudder servo ******************/
tnhnrl 56:48a8a5a65b82 56 Ticker systemTicker;
tnhnrl 56:48a8a5a65b82 57
tnhnrl 56:48a8a5a65b82 58 volatile unsigned int timer_counter = 0;
tnhnrl 56:48a8a5a65b82 59
tnhnrl 56:48a8a5a65b82 60 bool setup_complete = false;
tnhnrl 56:48a8a5a65b82 61
tnhnrl 56:48a8a5a65b82 62 static void system_timer(void) {
tnhnrl 56:48a8a5a65b82 63 timer_counter++;
tnhnrl 56:48a8a5a65b82 64
tnhnrl 56:48a8a5a65b82 65 //only start these updates when everything is properly setup (through setup function)
tnhnrl 56:48a8a5a65b82 66 if (setup_complete) {
tnhnrl 56:48a8a5a65b82 67 if ( (timer_counter % 20) == 0 ) { // 0.02 second intervals
tnhnrl 56:48a8a5a65b82 68 rudder().runServo();
tnhnrl 56:48a8a5a65b82 69 }
tnhnrl 56:48a8a5a65b82 70 }
tnhnrl 56:48a8a5a65b82 71 }
tnhnrl 56:48a8a5a65b82 72
tnhnrl 56:48a8a5a65b82 73 /****************** for rudder servo ******************/
tnhnrl 56:48a8a5a65b82 74
danstrider 10:085ab7328054 75 void setup() {
danstrider 11:3b241ecb75ed 76 pc().baud(57600);
tnhnrl 61:f7437daae608 77 pc().printf("\n\n\rFSG PCB V1.4 2018-06-18 \n\n\r");
mkelly10 51:c5c40272ecc3 78
mkelly10 51:c5c40272ecc3 79 /* //setup data logger baud rate and write the start of the program (every time you reset)
tnhnrl 32:f2f8ae34aadc 80 datalogger().baud(57600);
tnhnrl 39:58375ca6b6ff 81 datalogger().printf("SYSTEM, RESET\n");
mkelly10 51:c5c40272ecc3 82 */
mkelly10 51:c5c40272ecc3 83
danstrider 10:085ab7328054 84 // start up the system timer
tzyoung 0:ea293bbf9717 85 systemTime().start();
tnhnrl 20:8987a9ae2bc7 86
danstrider 10:085ab7328054 87 // set up and start the adc. This runs on a fixed interval and is interrupt driven
mkelly10 51:c5c40272ecc3 88 adc().initialize();
tzyoung 0:ea293bbf9717 89 adc().start();
danstrider 10:085ab7328054 90
danstrider 10:085ab7328054 91 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 92 imu().initialize();
danstrider 10:085ab7328054 93 imu().start();
danstrider 10:085ab7328054 94
danstrider 10:085ab7328054 95 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 14:85b64a4d08e8 96 depth().init();
danstrider 14:85b64a4d08e8 97 depth().tare();
danstrider 10:085ab7328054 98
tnhnrl 48:20e681885161 99 // construct the MBED local file system
danstrider 10:085ab7328054 100 local();
tnhnrl 48:20e681885161 101
tnhnrl 48:20e681885161 102 // construct the SD card file system
mkelly10 51:c5c40272ecc3 103 // sd_card();
tnhnrl 20:8987a9ae2bc7 104
danstrider 10:085ab7328054 105 // load config data from files
tnhnrl 21:38c8544db6f4 106 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 21:38c8544db6f4 107 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 21:38c8544db6f4 108 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 21:38c8544db6f4 109 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 20:8987a9ae2bc7 110
tnhnrl 43:891baf306e0a 111
danstrider 10:085ab7328054 112 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 113 bce().init();
danstrider 10:085ab7328054 114 bce().start();
mkelly10 12:a0519d11d2b6 115 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 116
danstrider 10:085ab7328054 117 batt().init();
tzyoung 2:892b58e56712 118 batt().start();
mkelly10 12:a0519d11d2b6 119 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 120
danstrider 10:085ab7328054 121 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 122 depthLoop().init();
tnhnrl 13:84fcbe1dcd62 123 depthLoop().start();
tnhnrl 16:3363b9f14913 124 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 125
danstrider 10:085ab7328054 126 pitchLoop().init();
tnhnrl 13:84fcbe1dcd62 127 pitchLoop().start();
tnhnrl 16:3363b9f14913 128 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 55:f4ec445c42fe 129
tnhnrl 55:f4ec445c42fe 130 //new 6/8/2018 for testing
tnhnrl 55:f4ec445c42fe 131 headingLoop().init();
tnhnrl 55:f4ec445c42fe 132 headingLoop().setCommand(0.0); //setting heading to 0 for testing
tnhnrl 55:f4ec445c42fe 133 //new 6/8/2018 for testing
tnhnrl 20:8987a9ae2bc7 134
tnhnrl 20:8987a9ae2bc7 135 // show that the PID gains are loading from the file
tnhnrl 38:83d06c294807 136 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 137 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 138 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 139 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 140 pc().printf("\n\r");
tnhnrl 21:38c8544db6f4 141
tnhnrl 17:7c16b5671d0e 142 //load sequence from file
tnhnrl 17:7c16b5671d0e 143 sequenceController().loadSequence();
tnhnrl 20:8987a9ae2bc7 144
tnhnrl 20:8987a9ae2bc7 145 // establish the main loop rate
tnhnrl 20:8987a9ae2bc7 146 loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
tnhnrl 32:f2f8ae34aadc 147
tnhnrl 32:f2f8ae34aadc 148 // setup the data logger rate
tnhnrl 34:9b66c5188051 149 log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
mkelly10 51:c5c40272ecc3 150
tnhnrl 47:fb3c7929d3f3 151 //set time of logger (to current or close-to-current time)
mkelly10 51:c5c40272ecc3 152 mbedLogger().setLogTime();
mkelly10 51:c5c40272ecc3 153 // sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 154
tnhnrl 47:fb3c7929d3f3 155 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 156 mbedLogger().initializeLogFile();
mkelly10 51:c5c40272ecc3 157 // sdLogger().initializeLogFile();
mkelly10 51:c5c40272ecc3 158
tnhnrl 58:94b7fd55185e 159 //tare the pressure sensor during setup
tnhnrl 58:94b7fd55185e 160 depth().tare();
tnhnrl 58:94b7fd55185e 161
tnhnrl 56:48a8a5a65b82 162 setup_complete = true; //used for interrupt timing
tnhnrl 56:48a8a5a65b82 163
tnhnrl 21:38c8544db6f4 164 }
tnhnrl 20:8987a9ae2bc7 165
danstrider 10:085ab7328054 166 int main() {
danstrider 10:085ab7328054 167 setup();
tnhnrl 56:48a8a5a65b82 168
tnhnrl 56:48a8a5a65b82 169 //this is used to control timing on the rudder servo, it's based on the new timing model in the 5/29/18 code with one interrupt running everything...may or may not work here
tnhnrl 56:48a8a5a65b82 170
tnhnrl 56:48a8a5a65b82 171 systemTicker.attach_us(&system_timer, 1000); // Interrupt timer running at 0.001 seconds (slower than original ADC time interval)
mkelly10 51:c5c40272ecc3 172
danstrider 10:085ab7328054 173 while(1) {
tnhnrl 34:9b66c5188051 174 static int current_state = 0;
tnhnrl 36:966a86937e17 175
tnhnrl 36:966a86937e17 176 static bool file_opened = false;
tnhnrl 36:966a86937e17 177
tnhnrl 39:58375ca6b6ff 178 // FSM loop runs at 10 hz
tnhnrl 20:8987a9ae2bc7 179 if(loop) {
tnhnrl 32:f2f8ae34aadc 180 led1() = !led1(); // blink led 1
tnhnrl 45:16b8162188ca 181 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 20:8987a9ae2bc7 182 loop = false; // wait until the loop rate timer fires again
tnhnrl 17:7c16b5671d0e 183 }
tnhnrl 32:f2f8ae34aadc 184
tnhnrl 39:58375ca6b6ff 185 // log loop runs at 1 hz
tnhnrl 34:9b66c5188051 186 if (log_loop) {
tnhnrl 45:16b8162188ca 187 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 63:6cb0405fc6e6 188 if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 45:16b8162188ca 189 //main_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 190 //log_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 191
tnhnrl 45:16b8162188ca 192 ; //pass
tnhnrl 45:16b8162188ca 193 }
tnhnrl 34:9b66c5188051 194
tnhnrl 45:16b8162188ca 195 else if(current_state != 0) {
tnhnrl 45:16b8162188ca 196 if (!file_opened) { //if the log file is not open, open it
tnhnrl 45:16b8162188ca 197 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 52:f207567d3ea4 198 // sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 45:16b8162188ca 199
tnhnrl 45:16b8162188ca 200 file_opened = true; //stops it from continuing to open it
tnhnrl 37:357e98a929cc 201
tnhnrl 36:966a86937e17 202 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 203 }
tnhnrl 36:966a86937e17 204
tnhnrl 45:16b8162188ca 205 //record to Mbed file system
tnhnrl 45:16b8162188ca 206 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 52:f207567d3ea4 207 // sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 36:966a86937e17 208 }
tnhnrl 36:966a86937e17 209
tnhnrl 39:58375ca6b6ff 210 //when the current FSM state is zero, reset the file
tnhnrl 36:966a86937e17 211 else {
tnhnrl 36:966a86937e17 212 //this can only happen once
tnhnrl 36:966a86937e17 213 if (file_opened) {
tnhnrl 45:16b8162188ca 214 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 52:f207567d3ea4 215 // sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 45:16b8162188ca 216
tnhnrl 36:966a86937e17 217 file_opened = false;
tnhnrl 37:357e98a929cc 218
tnhnrl 37:357e98a929cc 219 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 220 }
tnhnrl 34:9b66c5188051 221 }
tnhnrl 34:9b66c5188051 222
tnhnrl 34:9b66c5188051 223 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 45:16b8162188ca 224 } //END OF LOG LOOP
danstrider 10:085ab7328054 225 }
mkelly10 51:c5c40272ecc3 226
danstrider 10:085ab7328054 227 }