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@79:3688c3a0d7f4, 2018-10-23 (annotated)

Committer:
tnhnrl
Date:
Tue Oct 23 20:35:16 2018 +0000
Revision:
79:3688c3a0d7f4
Parent:
75:92e79d23d29a
Child:
80:4e5d306d695b
SD card logger working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tzyoung 0:ea293bbf9717 1 #include "mbed.h"
tzyoung 0:ea293bbf9717 2 #include "StaticDefs.hpp"
tnhnrl 65:2ac186553959 3
tnhnrl 65:2ac186553959 4 ////////////////////////////////////////////////////////////////// NEW TICKER
tnhnrl 65:2ac186553959 5 Ticker systemTicker;
tnhnrl 65:2ac186553959 6 bool setup_complete = false;
tnhnrl 65:2ac186553959 7 volatile unsigned int bTick = 0;
tnhnrl 65:2ac186553959 8 volatile unsigned int timer_counter = 0;
tnhnrl 65:2ac186553959 9
tnhnrl 79:3688c3a0d7f4 10 FILE *_sd_fp; //new 10/23/18
tnhnrl 79:3688c3a0d7f4 11
tnhnrl 65:2ac186553959 12 static unsigned int read_ticker(void) { //Basically this makes sure you're reading the data at one instance (not while it's changing)
tnhnrl 65:2ac186553959 13 unsigned int val = bTick;
tnhnrl 65:2ac186553959 14 if( val )
tnhnrl 65:2ac186553959 15 bTick = 0;
tnhnrl 65:2ac186553959 16 return( val );
tnhnrl 65:2ac186553959 17 }
tnhnrl 65:2ac186553959 18 ////////////////////////////////////////////////////////////////// NEW TICKER
tnhnrl 20:8987a9ae2bc7 19
tnhnrl 32:f2f8ae34aadc 20 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 65:2ac186553959 21 //Ticker main_loop_rate_ticker;
tnhnrl 65:2ac186553959 22 //Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 23
tnhnrl 65:2ac186553959 24 volatile bool fsm_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 65:2ac186553959 27 void loop_trigger() { fsm_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
tnhnrl 65:2ac186553959 30 static int current_state = 0;
tnhnrl 65:2ac186553959 31 static bool file_opened = false;
tnhnrl 65:2ac186553959 32
tnhnrl 73:f6f378311c8d 33 void FSM() { // FSM loop runs at 10 hz
tnhnrl 65:2ac186553959 34 if(fsm_loop) {
tnhnrl 73:f6f378311c8d 35 // led one removed
tnhnrl 65:2ac186553959 36 fsm_loop = false; // wait until the loop rate timer fires again
tnhnrl 65:2ac186553959 37 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 65:2ac186553959 38 }
tnhnrl 65:2ac186553959 39 }
tnhnrl 65:2ac186553959 40
tnhnrl 65:2ac186553959 41 void log_function() {
tnhnrl 65:2ac186553959 42 // log loop runs at 1 hz
tnhnrl 65:2ac186553959 43 if (log_loop) {
tnhnrl 65:2ac186553959 44 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 65:2ac186553959 45
tnhnrl 68:8f549749b8ce 46 //if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 68:8f549749b8ce 47 // ; //pass
tnhnrl 68:8f549749b8ce 48 // }
tnhnrl 65:2ac186553959 49
tnhnrl 68:8f549749b8ce 50 if(current_state != 0) {
tnhnrl 65:2ac186553959 51 if (!file_opened) { //if the log file is not open, open it
tnhnrl 65:2ac186553959 52 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 79:3688c3a0d7f4 53 sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 65:2ac186553959 54
tnhnrl 65:2ac186553959 55 file_opened = true; //stops it from continuing to open it
tnhnrl 56:48a8a5a65b82 56
tnhnrl 74:d281aaef9766 57 xbee().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 65:2ac186553959 58 }
tnhnrl 65:2ac186553959 59
tnhnrl 65:2ac186553959 60 //record to Mbed file system
tnhnrl 65:2ac186553959 61
tnhnrl 65:2ac186553959 62 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 79:3688c3a0d7f4 63 sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 65:2ac186553959 64 }
tnhnrl 65:2ac186553959 65
tnhnrl 65:2ac186553959 66 //when the current FSM state is zero (SIT_IDLE), close the file
tnhnrl 65:2ac186553959 67 else {
tnhnrl 65:2ac186553959 68 //this can only happen once
tnhnrl 65:2ac186553959 69 if (file_opened) {
tnhnrl 67:c86a4b464682 70 //WRITE ONCE
tnhnrl 67:c86a4b464682 71 mbedLogger().appendLogFile(current_state, 1); //write the idle state, then close
tnhnrl 79:3688c3a0d7f4 72 sdLogger().appendLogFile(current_state, 1); //write the idle state, then close
tnhnrl 67:c86a4b464682 73
tnhnrl 65:2ac186553959 74 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 79:3688c3a0d7f4 75 sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 65:2ac186553959 76
tnhnrl 65:2ac186553959 77 file_opened = false;
tnhnrl 65:2ac186553959 78
tnhnrl 74:d281aaef9766 79 xbee().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 65:2ac186553959 80 }
tnhnrl 65:2ac186553959 81 }
tnhnrl 74:d281aaef9766 82 } //END OF LOG LOOP8
tnhnrl 65:2ac186553959 83
tnhnrl 65:2ac186553959 84 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 65:2ac186553959 85 }
tnhnrl 56:48a8a5a65b82 86
tnhnrl 72:250b2665755c 87 //single system timer to run hardware/electronics timing
tnhnrl 65:2ac186553959 88 static void system_timer(void) {
tnhnrl 65:2ac186553959 89 bTick = 1;
tnhnrl 65:2ac186553959 90
tnhnrl 56:48a8a5a65b82 91 timer_counter++;
tnhnrl 56:48a8a5a65b82 92
tnhnrl 56:48a8a5a65b82 93 //only start these updates when everything is properly setup (through setup function)
tnhnrl 56:48a8a5a65b82 94 if (setup_complete) {
tnhnrl 74:d281aaef9766 95 if ( (timer_counter % 5) == 0) { //this runs at 0.005 second intervals (200 Hz)
tnhnrl 65:2ac186553959 96 adc().update(); //every iteration of this the A/D converter runs //now this runs at 0.01 second intervals 03/12/2018
tnhnrl 65:2ac186553959 97 }
tnhnrl 65:2ac186553959 98
tnhnrl 65:2ac186553959 99 if ( (timer_counter % 10) == 0) {
tnhnrl 65:2ac186553959 100 bce().update(); //update() inside LinearActuator class (running at 0.01 second intervals)
tnhnrl 65:2ac186553959 101 batt().update();
tnhnrl 65:2ac186553959 102 }
tnhnrl 65:2ac186553959 103
tnhnrl 65:2ac186553959 104 if ( (timer_counter % 20) == 0 ) { // 0.02 second intervals
tnhnrl 74:d281aaef9766 105 rudder().runServo();
tnhnrl 65:2ac186553959 106 }
tnhnrl 65:2ac186553959 107
tnhnrl 65:2ac186553959 108 if ( (timer_counter % 50) == 0 ) { // 0.05 second intervals
tnhnrl 67:c86a4b464682 109 imu().runIMU();
tnhnrl 65:2ac186553959 110 }
tnhnrl 73:f6f378311c8d 111
tnhnrl 65:2ac186553959 112 if ( (timer_counter % 100) == 0) { // 100,000 microseconds = 0.1 second intervals
tnhnrl 65:2ac186553959 113 depthLoop().runOuterLoop();
tnhnrl 65:2ac186553959 114 pitchLoop().runOuterLoop();
tnhnrl 65:2ac186553959 115 headingLoop().runOuterLoop();
tnhnrl 65:2ac186553959 116 }
tnhnrl 73:f6f378311c8d 117
tnhnrl 73:f6f378311c8d 118 if ( (timer_counter % 1000) == 0) { // update at 1.0 second intervals
tnhnrl 73:f6f378311c8d 119 //gui().updateGUI();
tnhnrl 73:f6f378311c8d 120 }
tnhnrl 74:d281aaef9766 121
tnhnrl 74:d281aaef9766 122 if ( (timer_counter % 30000) == 0) { // update at 30.0 second intervals
tnhnrl 74:d281aaef9766 123 //pc().printf("XB!\n");
tnhnrl 74:d281aaef9766 124 }
tnhnrl 56:48a8a5a65b82 125 }
tnhnrl 56:48a8a5a65b82 126 }
tnhnrl 56:48a8a5a65b82 127
danstrider 10:085ab7328054 128 void setup() {
tnhnrl 74:d281aaef9766 129 xbee().baud(115200);
tnhnrl 75:92e79d23d29a 130 xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee\n\n\r");
tnhnrl 74:d281aaef9766 131
tnhnrl 74:d281aaef9766 132 //pc().baud(57600);
tnhnrl 65:2ac186553959 133
danstrider 10:085ab7328054 134 // start up the system timer
tnhnrl 65:2ac186553959 135 //systemTimer().start();
tnhnrl 20:8987a9ae2bc7 136
danstrider 10:085ab7328054 137 // set up and start the adc. This runs on a fixed interval and is interrupt driven
tnhnrl 65:2ac186553959 138 adc().initialize();
tnhnrl 67:c86a4b464682 139 //one central interrupt is updating the ADC (not using the start function)
tnhnrl 65:2ac186553959 140
tnhnrl 65:2ac186553959 141 // setup and run the rudder(servo) pwm signal (start the ticker)
tnhnrl 65:2ac186553959 142 //rudder().init();
tnhnrl 74:d281aaef9766 143 xbee().printf("Rudder servo initialized!\n\r");
danstrider 10:085ab7328054 144
danstrider 10:085ab7328054 145 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 146 imu().initialize();
tnhnrl 65:2ac186553959 147 //imu().start();
danstrider 10:085ab7328054 148
tnhnrl 48:20e681885161 149 // construct the MBED local file system
danstrider 10:085ab7328054 150 local();
tnhnrl 48:20e681885161 151
tnhnrl 79:3688c3a0d7f4 152 // construct the SD card file system TEST 10/23/18
tnhnrl 79:3688c3a0d7f4 153 sd_card();
tnhnrl 20:8987a9ae2bc7 154
danstrider 10:085ab7328054 155 // load config data from files
tnhnrl 65:2ac186553959 156 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 65:2ac186553959 157 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 65:2ac186553959 158
tnhnrl 65:2ac186553959 159 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 65:2ac186553959 160 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 65:2ac186553959 161
tnhnrl 65:2ac186553959 162 configFileIO().load_RUDDER_config(); // load the rudder servo inner loop parameters from the file "SERVO.txt"
tnhnrl 65:2ac186553959 163 configFileIO().load_HEADING_config(); // load the rudder servo outer loop HEADING control parameters from the file "HEADING.txt" (contains neutral position)
tnhnrl 43:891baf306e0a 164
danstrider 10:085ab7328054 165 // set up the linear actuators. adc has to be running first.
tnhnrl 65:2ac186553959 166 bce().setPIDHighLimit(bce().getTravelLimit()); //travel limit of this linear actuator
danstrider 10:085ab7328054 167 bce().init();
tnhnrl 65:2ac186553959 168 //bce().start(); //removed start, it's handled by the interrupt
tnhnrl 67:c86a4b464682 169 bce().runLinearActuator();
mkelly10 12:a0519d11d2b6 170 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 171
tnhnrl 65:2ac186553959 172 batt().setPIDHighLimit(batt().getTravelLimit()); //travel limit of this linear actuator
danstrider 10:085ab7328054 173 batt().init();
tnhnrl 65:2ac186553959 174 batt().runLinearActuator(); // _init = true;
tnhnrl 65:2ac186553959 175 //batt().start();//removed start, it's handled by the interrupt
mkelly10 12:a0519d11d2b6 176 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 177
tnhnrl 65:2ac186553959 178 // set up the depth, pitch, and rudder outer loop controllers
danstrider 10:085ab7328054 179 depthLoop().init();
tnhnrl 65:2ac186553959 180 //removed start, it's handled by the interrupt
tnhnrl 16:3363b9f14913 181 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 182
danstrider 10:085ab7328054 183 pitchLoop().init();
tnhnrl 65:2ac186553959 184 //removed start, it's handled by the interrupt
tnhnrl 16:3363b9f14913 185 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 55:f4ec445c42fe 186
tnhnrl 55:f4ec445c42fe 187 headingLoop().init();
tnhnrl 65:2ac186553959 188 //removed start, it's handled by the interrupt
tnhnrl 65:2ac186553959 189 //headingLoop().setCommand(stateMachine().getHeadingCommand()); // FIX LATER
tnhnrl 65:2ac186553959 190 //heading flag that adjust the PID error is set in the constructor
tnhnrl 65:2ac186553959 191
tnhnrl 65:2ac186553959 192 //systemTicker.attach_us(&system_timer, 10000); // Interrupt timer running at 0.01 seconds (slower than original ADC time interval)
tnhnrl 65:2ac186553959 193
tnhnrl 65:2ac186553959 194
tnhnrl 20:8987a9ae2bc7 195
tnhnrl 20:8987a9ae2bc7 196 // show that the PID gains are loading from the file
tnhnrl 74:d281aaef9766 197 xbee().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 74:d281aaef9766 198 xbee().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 74:d281aaef9766 199 xbee().printf("rudder min pwm: %6.2f, max pwm: %6.2f, center pwm: %6.2f, min deg: %6.2f, max deg: %6.2f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
tnhnrl 65:2ac186553959 200
tnhnrl 74:d281aaef9766 201 xbee().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 74:d281aaef9766 202 xbee().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());
tnhnrl 74:d281aaef9766 203 xbee().printf("heading P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f deg (deadband: %0.1f)\r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset(), headingLoop().getDeadband());
tnhnrl 65:2ac186553959 204
tnhnrl 74:d281aaef9766 205 xbee().printf("\n\r");
tnhnrl 21:38c8544db6f4 206
tnhnrl 17:7c16b5671d0e 207 //load sequence from file
tnhnrl 17:7c16b5671d0e 208 sequenceController().loadSequence();
tnhnrl 32:f2f8ae34aadc 209
tnhnrl 47:fb3c7929d3f3 210 //set time of logger (to current or close-to-current time)
tnhnrl 65:2ac186553959 211 mbedLogger().setLogTime();
tnhnrl 65:2ac186553959 212 //sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 213
tnhnrl 47:fb3c7929d3f3 214 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 215 mbedLogger().initializeLogFile();
tnhnrl 79:3688c3a0d7f4 216 sdLogger().initializeLogFile(); // works 10/23/18
tnhnrl 65:2ac186553959 217
tnhnrl 68:8f549749b8ce 218 setup_complete = true;
tnhnrl 65:2ac186553959 219 }
tnhnrl 58:94b7fd55185e 220
tnhnrl 68:8f549749b8ce 221 /*************************** v1.8 **************************/
tnhnrl 68:8f549749b8ce 222
tnhnrl 73:f6f378311c8d 223 int main() {
tnhnrl 73:f6f378311c8d 224 setup(); //setup electronics/hardware
tnhnrl 56:48a8a5a65b82 225
tnhnrl 72:250b2665755c 226 systemTicker.attach_us(&system_timer, 1000); // Interrupt timer running at 0.001 seconds (slower than original ADC time interval)
tnhnrl 68:8f549749b8ce 227
tnhnrl 65:2ac186553959 228 unsigned int tNow = 0;
tnhnrl 74:d281aaef9766 229
tnhnrl 75:92e79d23d29a 230 xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (setup complete) \n\n\r");
tnhnrl 56:48a8a5a65b82 231
tnhnrl 56:48a8a5a65b82 232 systemTicker.attach_us(&system_timer, 1000); // Interrupt timer running at 0.001 seconds (slower than original ADC time interval)
tnhnrl 65:2ac186553959 233
tnhnrl 65:2ac186553959 234 while (1) {
tnhnrl 66:0f20870117b7 235 if( read_ticker() ) // read_ticker runs at the speed of 10 kHz (adc timing)
tnhnrl 65:2ac186553959 236 {
tnhnrl 65:2ac186553959 237 ++tNow;
mkelly10 51:c5c40272ecc3 238
tnhnrl 66:0f20870117b7 239 //run finite state machine fast when transmitting data
tnhnrl 74:d281aaef9766 240 if (current_state == TX_MBED_LOG or current_state == RX_SEQUENCE) {
tnhnrl 73:f6f378311c8d 241 if ( (tNow % 100) == 0 ) { // 0.01 second intervals (100 Hz)
tnhnrl 66:0f20870117b7 242 fsm_loop = true;
tnhnrl 66:0f20870117b7 243 FSM();
tnhnrl 66:0f20870117b7 244 }
tnhnrl 45:16b8162188ca 245 }
tnhnrl 34:9b66c5188051 246
tnhnrl 67:c86a4b464682 247 //NOT TRANSMITTING DATA, NORMAL OPERATIONS
tnhnrl 68:8f549749b8ce 248 else {
tnhnrl 68:8f549749b8ce 249 //FSM
tnhnrl 68:8f549749b8ce 250 if ( (tNow % 100) == 0 ) { // 0.1 second intervals
tnhnrl 66:0f20870117b7 251 fsm_loop = true;
tnhnrl 66:0f20870117b7 252 FSM();
tnhnrl 73:f6f378311c8d 253
tnhnrl 73:f6f378311c8d 254 //get commands and update GUI
tnhnrl 73:f6f378311c8d 255 gui().getCommandFSM();
tnhnrl 68:8f549749b8ce 256 }
tnhnrl 68:8f549749b8ce 257 //LOGGING
tnhnrl 68:8f549749b8ce 258 if ( (tNow % 1000) == 0 ) { // 1.0 second intervals
tnhnrl 66:0f20870117b7 259 log_loop = true;
tnhnrl 66:0f20870117b7 260 log_function();
tnhnrl 66:0f20870117b7 261 }
tnhnrl 66:0f20870117b7 262 }
tnhnrl 65:2ac186553959 263 }
danstrider 10:085ab7328054 264 }
tnhnrl 74:d281aaef9766 265 }