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@58:94b7fd55185e, 2018-06-15 (annotated)

Committer:
tnhnrl
Date:
Fri Jun 15 20:11:39 2018 +0000
Revision:
58:94b7fd55185e
Parent:
57:ec69651c8c21
Child:
61:f7437daae608
For checking the menu only.

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 56:48a8a5a65b82 37
danstrider 10:085ab7328054 38 */
tnhnrl 20:8987a9ae2bc7 39
tzyoung 0:ea293bbf9717 40 #include "mbed.h"
tzyoung 0:ea293bbf9717 41 #include "StaticDefs.hpp"
tnhnrl 20:8987a9ae2bc7 42
tnhnrl 32:f2f8ae34aadc 43 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 20:8987a9ae2bc7 44 Ticker loop_rate_ticker;
tnhnrl 34:9b66c5188051 45 Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 46
tnhnrl 32:f2f8ae34aadc 47 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 48 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 49
tnhnrl 32:f2f8ae34aadc 50 void loop_trigger() { loop = true;} // loop trigger (used in while loop)
tnhnrl 34:9b66c5188051 51 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
tnhnrl 32:f2f8ae34aadc 52
tnhnrl 56:48a8a5a65b82 53 /****************** for rudder servo ******************/
tnhnrl 56:48a8a5a65b82 54 Ticker systemTicker;
tnhnrl 56:48a8a5a65b82 55
tnhnrl 56:48a8a5a65b82 56 volatile unsigned int timer_counter = 0;
tnhnrl 56:48a8a5a65b82 57
tnhnrl 56:48a8a5a65b82 58 bool setup_complete = false;
tnhnrl 56:48a8a5a65b82 59
tnhnrl 56:48a8a5a65b82 60 static void system_timer(void) {
tnhnrl 56:48a8a5a65b82 61 timer_counter++;
tnhnrl 56:48a8a5a65b82 62
tnhnrl 56:48a8a5a65b82 63 //only start these updates when everything is properly setup (through setup function)
tnhnrl 56:48a8a5a65b82 64 if (setup_complete) {
tnhnrl 56:48a8a5a65b82 65 if ( (timer_counter % 20) == 0 ) { // 0.02 second intervals
tnhnrl 56:48a8a5a65b82 66 rudder().runServo();
tnhnrl 56:48a8a5a65b82 67 }
tnhnrl 56:48a8a5a65b82 68 }
tnhnrl 56:48a8a5a65b82 69 }
tnhnrl 56:48a8a5a65b82 70
tnhnrl 56:48a8a5a65b82 71 /****************** for rudder servo ******************/
tnhnrl 56:48a8a5a65b82 72
danstrider 10:085ab7328054 73 void setup() {
danstrider 11:3b241ecb75ed 74 pc().baud(57600);
tnhnrl 52:f207567d3ea4 75 pc().printf("\n\n\rFSG PCB V1.5 2018-06-06 \n\n\r");
mkelly10 51:c5c40272ecc3 76
mkelly10 51:c5c40272ecc3 77 /* //setup data logger baud rate and write the start of the program (every time you reset)
tnhnrl 32:f2f8ae34aadc 78 datalogger().baud(57600);
tnhnrl 39:58375ca6b6ff 79 datalogger().printf("SYSTEM, RESET\n");
mkelly10 51:c5c40272ecc3 80 */
mkelly10 51:c5c40272ecc3 81
danstrider 10:085ab7328054 82 // start up the system timer
tzyoung 0:ea293bbf9717 83 systemTime().start();
tnhnrl 20:8987a9ae2bc7 84
danstrider 10:085ab7328054 85 // set up and start the adc. This runs on a fixed interval and is interrupt driven
mkelly10 51:c5c40272ecc3 86 adc().initialize();
tzyoung 0:ea293bbf9717 87 adc().start();
danstrider 10:085ab7328054 88
danstrider 10:085ab7328054 89 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 90 imu().initialize();
danstrider 10:085ab7328054 91 imu().start();
danstrider 10:085ab7328054 92
danstrider 10:085ab7328054 93 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 14:85b64a4d08e8 94 depth().init();
danstrider 14:85b64a4d08e8 95 depth().tare();
danstrider 10:085ab7328054 96
tnhnrl 48:20e681885161 97 // construct the MBED local file system
danstrider 10:085ab7328054 98 local();
tnhnrl 48:20e681885161 99
tnhnrl 48:20e681885161 100 // construct the SD card file system
mkelly10 51:c5c40272ecc3 101 // sd_card();
tnhnrl 20:8987a9ae2bc7 102
danstrider 10:085ab7328054 103 // load config data from files
tnhnrl 21:38c8544db6f4 104 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 21:38c8544db6f4 105 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 21:38c8544db6f4 106 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 21:38c8544db6f4 107 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 20:8987a9ae2bc7 108
tnhnrl 43:891baf306e0a 109
danstrider 10:085ab7328054 110 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 111 bce().init();
danstrider 10:085ab7328054 112 bce().start();
mkelly10 12:a0519d11d2b6 113 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 114
danstrider 10:085ab7328054 115 batt().init();
tzyoung 2:892b58e56712 116 batt().start();
mkelly10 12:a0519d11d2b6 117 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 118
danstrider 10:085ab7328054 119 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 120 depthLoop().init();
tnhnrl 13:84fcbe1dcd62 121 depthLoop().start();
tnhnrl 16:3363b9f14913 122 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 123
danstrider 10:085ab7328054 124 pitchLoop().init();
tnhnrl 13:84fcbe1dcd62 125 pitchLoop().start();
tnhnrl 16:3363b9f14913 126 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 55:f4ec445c42fe 127
tnhnrl 55:f4ec445c42fe 128 //new 6/8/2018 for testing
tnhnrl 55:f4ec445c42fe 129 headingLoop().init();
tnhnrl 55:f4ec445c42fe 130 headingLoop().setCommand(0.0); //setting heading to 0 for testing
tnhnrl 55:f4ec445c42fe 131 //new 6/8/2018 for testing
tnhnrl 20:8987a9ae2bc7 132
tnhnrl 20:8987a9ae2bc7 133 // show that the PID gains are loading from the file
tnhnrl 38:83d06c294807 134 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 135 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 136 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 137 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 138 pc().printf("\n\r");
tnhnrl 21:38c8544db6f4 139
tnhnrl 17:7c16b5671d0e 140 //load sequence from file
tnhnrl 17:7c16b5671d0e 141 sequenceController().loadSequence();
tnhnrl 20:8987a9ae2bc7 142
tnhnrl 20:8987a9ae2bc7 143 // establish the main loop rate
tnhnrl 20:8987a9ae2bc7 144 loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
tnhnrl 32:f2f8ae34aadc 145
tnhnrl 32:f2f8ae34aadc 146 // setup the data logger rate
tnhnrl 34:9b66c5188051 147 log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)
mkelly10 51:c5c40272ecc3 148
tnhnrl 47:fb3c7929d3f3 149 //set time of logger (to current or close-to-current time)
mkelly10 51:c5c40272ecc3 150 mbedLogger().setLogTime();
mkelly10 51:c5c40272ecc3 151 // sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 152
tnhnrl 47:fb3c7929d3f3 153 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 154 mbedLogger().initializeLogFile();
mkelly10 51:c5c40272ecc3 155 // sdLogger().initializeLogFile();
mkelly10 51:c5c40272ecc3 156
tnhnrl 58:94b7fd55185e 157 //tare the pressure sensor during setup
tnhnrl 58:94b7fd55185e 158 depth().tare();
tnhnrl 58:94b7fd55185e 159
tnhnrl 56:48a8a5a65b82 160 setup_complete = true; //used for interrupt timing
tnhnrl 56:48a8a5a65b82 161
tnhnrl 21:38c8544db6f4 162 }
tnhnrl 20:8987a9ae2bc7 163
danstrider 10:085ab7328054 164 int main() {
danstrider 10:085ab7328054 165 setup();
tnhnrl 56:48a8a5a65b82 166
tnhnrl 56:48a8a5a65b82 167 //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 168
tnhnrl 56:48a8a5a65b82 169 systemTicker.attach_us(&system_timer, 1000); // Interrupt timer running at 0.001 seconds (slower than original ADC time interval)
mkelly10 51:c5c40272ecc3 170
danstrider 10:085ab7328054 171 while(1) {
tnhnrl 34:9b66c5188051 172 static int current_state = 0;
tnhnrl 36:966a86937e17 173
tnhnrl 36:966a86937e17 174 static bool file_opened = false;
tnhnrl 36:966a86937e17 175
tnhnrl 39:58375ca6b6ff 176 // FSM loop runs at 10 hz
tnhnrl 20:8987a9ae2bc7 177 if(loop) {
tnhnrl 32:f2f8ae34aadc 178 led1() = !led1(); // blink led 1
tnhnrl 45:16b8162188ca 179 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 20:8987a9ae2bc7 180 loop = false; // wait until the loop rate timer fires again
tnhnrl 17:7c16b5671d0e 181 }
tnhnrl 32:f2f8ae34aadc 182
tnhnrl 39:58375ca6b6ff 183 // log loop runs at 1 hz
tnhnrl 34:9b66c5188051 184 if (log_loop) {
tnhnrl 45:16b8162188ca 185 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 45:16b8162188ca 186 if (current_state == TRANSMIT_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 45:16b8162188ca 187 //main_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 188 //log_loop_rate_ticker.detach();
tnhnrl 45:16b8162188ca 189
tnhnrl 45:16b8162188ca 190 ; //pass
tnhnrl 45:16b8162188ca 191 }
tnhnrl 34:9b66c5188051 192
tnhnrl 45:16b8162188ca 193 else if(current_state != 0) {
tnhnrl 45:16b8162188ca 194 if (!file_opened) { //if the log file is not open, open it
tnhnrl 45:16b8162188ca 195 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 52:f207567d3ea4 196 // sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 45:16b8162188ca 197
tnhnrl 45:16b8162188ca 198 file_opened = true; //stops it from continuing to open it
tnhnrl 37:357e98a929cc 199
tnhnrl 36:966a86937e17 200 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 201 }
tnhnrl 36:966a86937e17 202
tnhnrl 45:16b8162188ca 203 //record to Mbed file system
tnhnrl 45:16b8162188ca 204 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 52:f207567d3ea4 205 // sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 36:966a86937e17 206 }
tnhnrl 36:966a86937e17 207
tnhnrl 39:58375ca6b6ff 208 //when the current FSM state is zero, reset the file
tnhnrl 36:966a86937e17 209 else {
tnhnrl 36:966a86937e17 210 //this can only happen once
tnhnrl 36:966a86937e17 211 if (file_opened) {
tnhnrl 45:16b8162188ca 212 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 52:f207567d3ea4 213 // sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 45:16b8162188ca 214
tnhnrl 36:966a86937e17 215 file_opened = false;
tnhnrl 37:357e98a929cc 216
tnhnrl 37:357e98a929cc 217 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 36:966a86937e17 218 }
tnhnrl 34:9b66c5188051 219 }
tnhnrl 34:9b66c5188051 220
tnhnrl 34:9b66c5188051 221 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 45:16b8162188ca 222 } //END OF LOG LOOP
danstrider 10:085ab7328054 223 }
mkelly10 51:c5c40272ecc3 224
danstrider 10:085ab7328054 225 }