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

StateMachine/StateMachine.cpp@45:16b8162188ca, 2018-02-15 (annotated)

Committer:
tnhnrl
Date:
Thu Feb 15 02:39:13 2018 +0000
Revision:
45:16b8162188ca
Parent:
39:58375ca6b6ff
Child:
49:47ffa4feb6db
version for USB testing with reduced class sizes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 16:3363b9f14913 1 #include "StateMachine.hpp"
tnhnrl 16:3363b9f14913 2 #include "StaticDefs.hpp"
tnhnrl 20:8987a9ae2bc7 3
tnhnrl 16:3363b9f14913 4 StateMachine::StateMachine() {
tnhnrl 37:357e98a929cc 5 _timeout = 120; // generic timeout for every state, seconds
tnhnrl 20:8987a9ae2bc7 6
tnhnrl 28:16c83a2fdefa 7 _pitchTolerance = 5.0; // pitch angle tolerance for FLOAT_LEVEL state
tnhnrl 20:8987a9ae2bc7 8
tnhnrl 28:16c83a2fdefa 9 _bceFloatPosition = bce().getTravelLimit(); // bce position for "float" states (max travel limit for BCE is 320 mm)
tnhnrl 28:16c83a2fdefa 10 _battFloatPosition = batt().getTravelLimit(); // batt position tail high for "broadcast" state (max travel limit for battery is 75 mm)
tnhnrl 20:8987a9ae2bc7 11
tnhnrl 32:f2f8ae34aadc 12 _depth_command = 2.0; // user keyboard depth (default)
tnhnrl 32:f2f8ae34aadc 13 _pitch_command = -20.0; // user keyboard pitch (default)
tnhnrl 17:7c16b5671d0e 14
tnhnrl 28:16c83a2fdefa 15 _neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM
tnhnrl 20:8987a9ae2bc7 16
tnhnrl 28:16c83a2fdefa 17 _state = SIT_IDLE; // select starting state here
tnhnrl 28:16c83a2fdefa 18 _isTimeoutRunning = false; // default timer to not running
tnhnrl 28:16c83a2fdefa 19 _isSubStateTimerRunning = false; // default timer to not running
tnhnrl 17:7c16b5671d0e 20
tnhnrl 24:c7d9b5bf3829 21 _multi_dive_counter = 0;
tnhnrl 21:38c8544db6f4 22
tnhnrl 21:38c8544db6f4 23 _neutral_sub_state_active = false;
tnhnrl 17:7c16b5671d0e 24
tnhnrl 21:38c8544db6f4 25 _depth_KP = depthLoop().getControllerP(); // load current depth value
tnhnrl 21:38c8544db6f4 26 _depth_KI = depthLoop().getControllerI(); // load current depth value
tnhnrl 21:38c8544db6f4 27 _depth_KD = depthLoop().getControllerD(); // load current depth value
tnhnrl 21:38c8544db6f4 28
tnhnrl 21:38c8544db6f4 29 _pitch_KP = pitchLoop().getControllerP(); // load current pitch value
tnhnrl 21:38c8544db6f4 30 _pitch_KI = pitchLoop().getControllerI(); // load current pitch value
tnhnrl 21:38c8544db6f4 31 _pitch_KD = pitchLoop().getControllerD(); // load current pitch value
tnhnrl 21:38c8544db6f4 32
tnhnrl 21:38c8544db6f4 33 _neutral_bce_pos_mm = depthLoop().getOutputOffset(); //load current neutral buoyancy position offset
tnhnrl 21:38c8544db6f4 34 _neutral_batt_pos_mm = pitchLoop().getOutputOffset(); //load current neutral buoyancy position offset
tnhnrl 23:434f04ef1fad 35
tnhnrl 28:16c83a2fdefa 36 _state_array_counter = 1; //used to iterate through and record states
tnhnrl 28:16c83a2fdefa 37 _substate_array_counter = 0; //used to iterate through and record substates
tnhnrl 28:16c83a2fdefa 38
tnhnrl 28:16c83a2fdefa 39 _state_array[0] = SIT_IDLE; //system starts in the SIT_IDLE state, record this
tnhnrl 24:c7d9b5bf3829 40
tnhnrl 30:2964617e7676 41 _substate = NEUTRAL_SINKING; //start sub-FSM in NEUTRAL_SINKING
tnhnrl 28:16c83a2fdefa 42 _previous_substate = -1; //to start sub-FSM
tnhnrl 28:16c83a2fdefa 43 _previous_state = -1; //for tracking FSM states
tnhnrl 28:16c83a2fdefa 44
tnhnrl 28:16c83a2fdefa 45 _max_recorded_depth_neutral = -99; //float to record max depth
tnhnrl 28:16c83a2fdefa 46 _max_recorded_depth_dive = -99; //float to record max depth
tnhnrl 32:f2f8ae34aadc 47
tnhnrl 32:f2f8ae34aadc 48 _neutral_sink_command_mm = -2.5; //defaults for neutral finding sub-FSM
tnhnrl 32:f2f8ae34aadc 49 _neutral_rise_command_mm = 2.0;
tnhnrl 32:f2f8ae34aadc 50 _neutral_pitch_command_mm = 0.5;
tnhnrl 32:f2f8ae34aadc 51
tnhnrl 32:f2f8ae34aadc 52 _max_recorded_auto_neutral_depth = -99;
tnhnrl 32:f2f8ae34aadc 53
tnhnrl 32:f2f8ae34aadc 54 _file_closed = true;
tnhnrl 16:3363b9f14913 55 }
tnhnrl 20:8987a9ae2bc7 56
tnhnrl 17:7c16b5671d0e 57 //Finite State Machine (FSM)
tnhnrl 45:16b8162188ca 58 int StateMachine::runStateMachine() {
tnhnrl 45:16b8162188ca 59 static int transmit_packet_number = 1; //for data transmission
tnhnrl 45:16b8162188ca 60
tnhnrl 16:3363b9f14913 61 // finite state machine ... each state has at least one exit criteria
tnhnrl 17:7c16b5671d0e 62 switch (_state) {
tnhnrl 16:3363b9f14913 63 case SIT_IDLE :
tnhnrl 28:16c83a2fdefa 64 case KEYBOARD:
tnhnrl 16:3363b9f14913 65 // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
tnhnrl 28:16c83a2fdefa 66 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 67 showMenu();
tnhnrl 16:3363b9f14913 68 pc().printf("\r\n\nstate: SIT_IDLE\r\n");
tnhnrl 28:16c83a2fdefa 69 _isTimeoutRunning = true;
tnhnrl 20:8987a9ae2bc7 70
tnhnrl 16:3363b9f14913 71 // what is active?
tnhnrl 16:3363b9f14913 72 bce().pause();
tnhnrl 16:3363b9f14913 73 batt().pause();
tnhnrl 32:f2f8ae34aadc 74
tnhnrl 17:7c16b5671d0e 75 //reset sub FSM
tnhnrl 28:16c83a2fdefa 76 _isSubStateTimerRunning = false;
tnhnrl 32:f2f8ae34aadc 77
tnhnrl 32:f2f8ae34aadc 78 //close the MBED file
tnhnrl 32:f2f8ae34aadc 79 _file_closed = true;
tnhnrl 16:3363b9f14913 80 }
tnhnrl 20:8987a9ae2bc7 81
tnhnrl 16:3363b9f14913 82 // how exit?
tnhnrl 20:8987a9ae2bc7 83 keyboard(); // keyboard function will change the state if needed
tnhnrl 16:3363b9f14913 84 break;
tnhnrl 20:8987a9ae2bc7 85
tnhnrl 16:3363b9f14913 86 case EMERGENCY_CLIMB :
tnhnrl 16:3363b9f14913 87 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 88 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 89 pc().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n");
tnhnrl 16:3363b9f14913 90 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 91 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 92 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 93
tnhnrl 16:3363b9f14913 94 // what needs to be started?
tnhnrl 16:3363b9f14913 95 bce().unpause();
tnhnrl 16:3363b9f14913 96 batt().unpause();
tnhnrl 20:8987a9ae2bc7 97
tnhnrl 20:8987a9ae2bc7 98 // what are the commands?
tnhnrl 16:3363b9f14913 99 bce().setPosition_mm(bce().getTravelLimit());
tnhnrl 16:3363b9f14913 100 batt().setPosition_mm(0.0);
tnhnrl 32:f2f8ae34aadc 101
tnhnrl 32:f2f8ae34aadc 102 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 103 ////////createNewFile();
tnhnrl 32:f2f8ae34aadc 104
tnhnrl 32:f2f8ae34aadc 105 //show that this is the start of a new EMERGENCY_CLIMB sequence
tnhnrl 32:f2f8ae34aadc 106 recordState(_state);
tnhnrl 32:f2f8ae34aadc 107
tnhnrl 32:f2f8ae34aadc 108 //this was missing
tnhnrl 32:f2f8ae34aadc 109 _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 110 recordData(_state);
tnhnrl 16:3363b9f14913 111 }
tnhnrl 20:8987a9ae2bc7 112
tnhnrl 16:3363b9f14913 113 // how exit?
tnhnrl 17:7c16b5671d0e 114 if (timer > _timeout) {
tnhnrl 16:3363b9f14913 115 pc().printf("EC: timed out\r\n");
tnhnrl 21:38c8544db6f4 116 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 117 timer.reset();
tnhnrl 28:16c83a2fdefa 118 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 119 }
tnhnrl 26:7e118fc02eea 120 else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
tnhnrl 21:38c8544db6f4 121 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 122 timer.reset();
tnhnrl 28:16c83a2fdefa 123 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 124 }
tnhnrl 32:f2f8ae34aadc 125
tnhnrl 34:9b66c5188051 126 //print status to screen continuously
tnhnrl 34:9b66c5188051 127 pc().printf("EC: depth: %3.1f (BCE_cmd: %0.1f), pitch: %0.1f deg [%0.1f sec]\r",depthLoop().getPosition(),bce().getPosition_mm(),pitchLoop().getPosition(),timer.read());
tnhnrl 34:9b66c5188051 128
tnhnrl 32:f2f8ae34aadc 129 //record data every 5 seconds
tnhnrl 34:9b66c5188051 130 recordData(_state);
tnhnrl 32:f2f8ae34aadc 131
tnhnrl 16:3363b9f14913 132 break;
tnhnrl 20:8987a9ae2bc7 133
tnhnrl 16:3363b9f14913 134 case FIND_NEUTRAL :
tnhnrl 20:8987a9ae2bc7 135 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 136 if (!_isTimeoutRunning) {
tnhnrl 21:38c8544db6f4 137 pc().printf("\r\n\nstate: FIND_NEUTRAL\n\r");
tnhnrl 16:3363b9f14913 138 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 139 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 140 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 141
tnhnrl 16:3363b9f14913 142 // what needs to be started?
tnhnrl 16:3363b9f14913 143 bce().unpause();
tnhnrl 16:3363b9f14913 144 batt().unpause();
tnhnrl 28:16c83a2fdefa 145 bce().setPosition_mm(_bceFloatPosition);
tnhnrl 24:c7d9b5bf3829 146 batt().setPosition_mm(_neutral_batt_pos_mm); //set battery to close-to-neutral setting from config file
tnhnrl 17:7c16b5671d0e 147
tnhnrl 24:c7d9b5bf3829 148 //first iteration goes into Neutral Finding Sub-FSM
tnhnrl 24:c7d9b5bf3829 149 //set the first state of the FSM, and start the sub-FSM
tnhnrl 30:2964617e7676 150 _substate = NEUTRAL_SINKING; //first state in neutral sub-FSM is the pressure vessel sinking
tnhnrl 28:16c83a2fdefa 151 _previous_substate = -1;
tnhnrl 28:16c83a2fdefa 152
tnhnrl 28:16c83a2fdefa 153 //save this state to the array
tnhnrl 30:2964617e7676 154 _substate_array[_substate_array_counter] = NEUTRAL_SINKING; //save to state array
tnhnrl 28:16c83a2fdefa 155 _substate_array_counter++;
tnhnrl 28:16c83a2fdefa 156
tnhnrl 32:f2f8ae34aadc 157 runNeutralStateMachine();
tnhnrl 32:f2f8ae34aadc 158
tnhnrl 32:f2f8ae34aadc 159 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 160 //createNewFile();
tnhnrl 32:f2f8ae34aadc 161
tnhnrl 32:f2f8ae34aadc 162 //show that this is the start of a new FIND_NEUTRAL sequence
tnhnrl 32:f2f8ae34aadc 163 recordState(_state);
tnhnrl 32:f2f8ae34aadc 164
tnhnrl 32:f2f8ae34aadc 165 //this was missing
tnhnrl 32:f2f8ae34aadc 166 _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 167 recordData(_state);
tnhnrl 16:3363b9f14913 168 }
tnhnrl 20:8987a9ae2bc7 169
tnhnrl 20:8987a9ae2bc7 170 // how exit? (exit with the timer, if timer still running continue processing sub FSM)
tnhnrl 17:7c16b5671d0e 171 if (timer > _timeout) {
tnhnrl 17:7c16b5671d0e 172 pc().printf("FN: timed out [time: %0.1f sec]\r\n", timer.read());
tnhnrl 21:38c8544db6f4 173 _state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces)
tnhnrl 16:3363b9f14913 174 timer.reset();
tnhnrl 28:16c83a2fdefa 175 _isTimeoutRunning = false;
tnhnrl 24:c7d9b5bf3829 176
tnhnrl 24:c7d9b5bf3829 177 //record this to the NEUTRAL sub-FSM tracker
tnhnrl 28:16c83a2fdefa 178 _substate_array[_substate_array_counter] = EMERGENCY_CLIMB; //save to state array
tnhnrl 28:16c83a2fdefa 179 _substate_array_counter++;
tnhnrl 16:3363b9f14913 180 }
tnhnrl 21:38c8544db6f4 181
tnhnrl 24:c7d9b5bf3829 182 //what is active? (neutral finding sub-function runs until completion)
tnhnrl 24:c7d9b5bf3829 183 //check if substate returned exit state, if so stop running the sub-FSM
tnhnrl 26:7e118fc02eea 184 else if (runNeutralStateMachine() == NEUTRAL_EXIT) {
tnhnrl 21:38c8544db6f4 185 //if successful, FIND_NEUTRAL then goes to RISE
tnhnrl 21:38c8544db6f4 186 pc().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\n\n\r");
tnhnrl 21:38c8544db6f4 187 _state = RISE;
tnhnrl 30:2964617e7676 188 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 189 }
tnhnrl 32:f2f8ae34aadc 190
tnhnrl 32:f2f8ae34aadc 191 //record data every 5 seconds
tnhnrl 34:9b66c5188051 192 recordData(_state);
tnhnrl 32:f2f8ae34aadc 193
tnhnrl 17:7c16b5671d0e 194 break;
tnhnrl 17:7c16b5671d0e 195
tnhnrl 16:3363b9f14913 196 case DIVE :
tnhnrl 16:3363b9f14913 197 // start local state timer and init any other one-shot actions
tnhnrl 32:f2f8ae34aadc 198
tnhnrl 28:16c83a2fdefa 199 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 200 pc().printf("\r\n\nstate: DIVE\r\n");
tnhnrl 16:3363b9f14913 201 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 202 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 203 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 204
tnhnrl 16:3363b9f14913 205 // what needs to be started?
tnhnrl 16:3363b9f14913 206 bce().unpause();
tnhnrl 16:3363b9f14913 207 batt().unpause();
tnhnrl 20:8987a9ae2bc7 208
tnhnrl 16:3363b9f14913 209 // what are the commands?
tnhnrl 32:f2f8ae34aadc 210 depthLoop().setCommand(_depth_command);
tnhnrl 32:f2f8ae34aadc 211 pitchLoop().setCommand(_pitch_command);
tnhnrl 32:f2f8ae34aadc 212
tnhnrl 34:9b66c5188051 213 pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
tnhnrl 34:9b66c5188051 214 pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 28:16c83a2fdefa 215
tnhnrl 28:16c83a2fdefa 216 //reset max dive depth
tnhnrl 28:16c83a2fdefa 217 _max_recorded_depth_dive = -99; //float to record max depth
tnhnrl 32:f2f8ae34aadc 218
tnhnrl 32:f2f8ae34aadc 219 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 220 //createNewFile();
tnhnrl 32:f2f8ae34aadc 221
tnhnrl 32:f2f8ae34aadc 222 //show that this is the start of new dive sequence
tnhnrl 32:f2f8ae34aadc 223 recordState(_state);
tnhnrl 32:f2f8ae34aadc 224
tnhnrl 32:f2f8ae34aadc 225 //this was missing
tnhnrl 32:f2f8ae34aadc 226 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 227 recordData(_state);
tnhnrl 16:3363b9f14913 228 }
tnhnrl 20:8987a9ae2bc7 229
tnhnrl 16:3363b9f14913 230 // how exit?
tnhnrl 32:f2f8ae34aadc 231 if (timer.read() > _timeout) {
tnhnrl 17:7c16b5671d0e 232 pc().printf("DIVE: timed out\n\n\r");
tnhnrl 21:38c8544db6f4 233 _state = RISE; //new behavior 11/17/2017
tnhnrl 16:3363b9f14913 234 timer.reset();
tnhnrl 28:16c83a2fdefa 235 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 236 }
tnhnrl 32:f2f8ae34aadc 237 else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches
tnhnrl 32:f2f8ae34aadc 238 pc().printf("DIVE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 21:38c8544db6f4 239 _state = RISE;
tnhnrl 16:3363b9f14913 240 timer.reset();
tnhnrl 28:16c83a2fdefa 241 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 242 }
tnhnrl 20:8987a9ae2bc7 243
tnhnrl 16:3363b9f14913 244 // what is active?
tnhnrl 21:38c8544db6f4 245 pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depth: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
tnhnrl 32:f2f8ae34aadc 246 bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
tnhnrl 16:3363b9f14913 247 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 28:16c83a2fdefa 248
tnhnrl 28:16c83a2fdefa 249 if (depthLoop().getPosition() > _max_recorded_depth_dive) { //debug
tnhnrl 28:16c83a2fdefa 250 _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded
tnhnrl 28:16c83a2fdefa 251 }
tnhnrl 32:f2f8ae34aadc 252
tnhnrl 32:f2f8ae34aadc 253 //record data every 5 seconds
tnhnrl 34:9b66c5188051 254 recordData(_state);
tnhnrl 32:f2f8ae34aadc 255
tnhnrl 16:3363b9f14913 256 break;
tnhnrl 16:3363b9f14913 257
tnhnrl 16:3363b9f14913 258 case RISE :
tnhnrl 16:3363b9f14913 259 // start local state timer and init any other one-shot actions
tnhnrl 32:f2f8ae34aadc 260
tnhnrl 28:16c83a2fdefa 261 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 262 pc().printf("\r\n\nstate: RISE\r\n");
tnhnrl 16:3363b9f14913 263 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 264 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 265 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 266
tnhnrl 16:3363b9f14913 267 // what needs to be started?
tnhnrl 16:3363b9f14913 268 bce().unpause();
tnhnrl 16:3363b9f14913 269 batt().unpause();
tnhnrl 16:3363b9f14913 270
tnhnrl 16:3363b9f14913 271 // what are the commands?
tnhnrl 28:16c83a2fdefa 272 depthLoop().setCommand(-1.0); //make sure to get towards the surface (saw issues at LASR pool)
tnhnrl 32:f2f8ae34aadc 273 pitchLoop().setCommand(-_pitch_command);
tnhnrl 34:9b66c5188051 274
tnhnrl 34:9b66c5188051 275 pc().printf("RISE: depth cmd: %0.1f\r\n",depthLoop().getCommand());
tnhnrl 34:9b66c5188051 276 pc().printf("RISE: pitch cmd: %0.1f\r\n",pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 277
tnhnrl 32:f2f8ae34aadc 278 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 279 //createNewFile();
tnhnrl 32:f2f8ae34aadc 280
tnhnrl 32:f2f8ae34aadc 281 //show that this is the start of new rise sequence
tnhnrl 32:f2f8ae34aadc 282 recordState(_state);
tnhnrl 32:f2f8ae34aadc 283
tnhnrl 32:f2f8ae34aadc 284 //this was missing
tnhnrl 32:f2f8ae34aadc 285 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 286 recordData(_state);
tnhnrl 16:3363b9f14913 287 }
tnhnrl 20:8987a9ae2bc7 288
tnhnrl 16:3363b9f14913 289 // how exit?
tnhnrl 32:f2f8ae34aadc 290 if (timer.read() > _timeout) {
tnhnrl 16:3363b9f14913 291 pc().printf("RISE: timed out\r\n");
tnhnrl 21:38c8544db6f4 292 _state = EMERGENCY_CLIMB;
tnhnrl 16:3363b9f14913 293 timer.reset();
tnhnrl 28:16c83a2fdefa 294 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 295 }
tnhnrl 32:f2f8ae34aadc 296
tnhnrl 32:f2f8ae34aadc 297 //modified from (depthLoop().getPosition() < depthLoop().getCommand() + 0.5)
tnhnrl 32:f2f8ae34aadc 298 //did not work correctly in bench test (stuck in rise state)
tnhnrl 32:f2f8ae34aadc 299 else if (depthLoop().getPosition() < 0.5) {
tnhnrl 32:f2f8ae34aadc 300 pc().printf("RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 28:16c83a2fdefa 301 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 302 timer.reset();
tnhnrl 28:16c83a2fdefa 303 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 304 }
tnhnrl 20:8987a9ae2bc7 305
tnhnrl 20:8987a9ae2bc7 306 // what is active?
tnhnrl 16:3363b9f14913 307 pc().printf("RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 17:7c16b5671d0e 308 bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
tnhnrl 32:f2f8ae34aadc 309 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 310
tnhnrl 32:f2f8ae34aadc 311 //record data every 5 seconds
tnhnrl 34:9b66c5188051 312 recordData(_state);
tnhnrl 32:f2f8ae34aadc 313
tnhnrl 16:3363b9f14913 314 break;
tnhnrl 16:3363b9f14913 315
tnhnrl 16:3363b9f14913 316 case FLOAT_LEVEL :
tnhnrl 16:3363b9f14913 317 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 318 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 319 pc().printf("\r\n\nstate: FLOAT_LEVEL\r\n");
tnhnrl 16:3363b9f14913 320 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 321 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 322 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 323
tnhnrl 16:3363b9f14913 324 // what needs to be started?
tnhnrl 16:3363b9f14913 325 bce().unpause();
tnhnrl 16:3363b9f14913 326 batt().unpause();
tnhnrl 16:3363b9f14913 327
tnhnrl 20:8987a9ae2bc7 328 // what are the commands?
tnhnrl 28:16c83a2fdefa 329 bce().setPosition_mm(_bceFloatPosition);
tnhnrl 16:3363b9f14913 330 pitchLoop().setCommand(0.0);
tnhnrl 32:f2f8ae34aadc 331
tnhnrl 32:f2f8ae34aadc 332 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 333 //createNewFile();
tnhnrl 32:f2f8ae34aadc 334
tnhnrl 32:f2f8ae34aadc 335 //show that this is the start of a new float level sequence
tnhnrl 32:f2f8ae34aadc 336 recordState(_state);
tnhnrl 32:f2f8ae34aadc 337
tnhnrl 32:f2f8ae34aadc 338 //this was missing
tnhnrl 32:f2f8ae34aadc 339 _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 340 recordData(_state);
tnhnrl 16:3363b9f14913 341 }
tnhnrl 20:8987a9ae2bc7 342
tnhnrl 16:3363b9f14913 343 // how exit?
tnhnrl 17:7c16b5671d0e 344 if (timer > _timeout) {
tnhnrl 16:3363b9f14913 345 pc().printf("FL: timed out\r\n");
tnhnrl 21:38c8544db6f4 346 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 347 timer.reset();
tnhnrl 28:16c83a2fdefa 348 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 349 }
tnhnrl 28:16c83a2fdefa 350 else if (fabs(imu().getPitch() - pitchLoop().getCommand()) < fabs(_pitchTolerance)) { //current tolerance is 5 degrees
tnhnrl 28:16c83a2fdefa 351 pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), _pitchTolerance);
tnhnrl 21:38c8544db6f4 352 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 353 timer.reset();
tnhnrl 28:16c83a2fdefa 354 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 355 }
tnhnrl 20:8987a9ae2bc7 356
tnhnrl 16:3363b9f14913 357 // what is active?
tnhnrl 16:3363b9f14913 358 pc().printf("FL: pitchLoop output: %3.1f, batt pos: %3.1f, piston pos: %3.1f [%0.1f sec]\r", pitchLoop().getOutput(), batt().getPosition_mm(), bce().getPosition_mm(), timer.read());
tnhnrl 16:3363b9f14913 359 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 360
tnhnrl 32:f2f8ae34aadc 361 //record data every 5 seconds
tnhnrl 34:9b66c5188051 362 recordData(_state);
tnhnrl 32:f2f8ae34aadc 363
tnhnrl 16:3363b9f14913 364 break;
tnhnrl 16:3363b9f14913 365
tnhnrl 16:3363b9f14913 366 case FLOAT_BROADCAST :
tnhnrl 16:3363b9f14913 367 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 368 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 369 pc().printf("\r\n\nstate: FLOAT_BROADCAST\r\n");
tnhnrl 16:3363b9f14913 370 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 371 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 372 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 373
tnhnrl 16:3363b9f14913 374 // what needs to be started?
tnhnrl 16:3363b9f14913 375 bce().unpause();
tnhnrl 16:3363b9f14913 376 batt().unpause();
tnhnrl 20:8987a9ae2bc7 377
tnhnrl 16:3363b9f14913 378 // what are the commands?
tnhnrl 28:16c83a2fdefa 379 bce().setPosition_mm(_bceFloatPosition);
tnhnrl 28:16c83a2fdefa 380 batt().setPosition_mm(_battFloatPosition);
tnhnrl 32:f2f8ae34aadc 381
tnhnrl 32:f2f8ae34aadc 382 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 383 //createNewFile();
tnhnrl 32:f2f8ae34aadc 384
tnhnrl 32:f2f8ae34aadc 385 //show that this is the start of a new float broadcast sequence
tnhnrl 32:f2f8ae34aadc 386 recordState(_state);
tnhnrl 32:f2f8ae34aadc 387
tnhnrl 32:f2f8ae34aadc 388 //this was missing
tnhnrl 32:f2f8ae34aadc 389 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 390 recordData(_state);
tnhnrl 16:3363b9f14913 391 }
tnhnrl 20:8987a9ae2bc7 392
tnhnrl 16:3363b9f14913 393 // how exit?
tnhnrl 17:7c16b5671d0e 394 if (timer > _timeout) {
tnhnrl 16:3363b9f14913 395 pc().printf("FB: timed out\r\n");
tnhnrl 21:38c8544db6f4 396 _state = SIT_IDLE;
tnhnrl 16:3363b9f14913 397 timer.reset();
tnhnrl 32:f2f8ae34aadc 398
tnhnrl 32:f2f8ae34aadc 399 //stop recording data
tnhnrl 35:2f66ea4863d5 400 //mbedLogger().closeFile();
tnhnrl 32:f2f8ae34aadc 401
tnhnrl 28:16c83a2fdefa 402 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 403 }
tnhnrl 20:8987a9ae2bc7 404 else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
tnhnrl 20:8987a9ae2bc7 405 (fabs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) {
tnhnrl 16:3363b9f14913 406 pc().printf("FB: position: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(), bce().getDeadband());
tnhnrl 21:38c8544db6f4 407 _state = SIT_IDLE;
tnhnrl 16:3363b9f14913 408 timer.reset();
tnhnrl 32:f2f8ae34aadc 409
tnhnrl 32:f2f8ae34aadc 410 //stop recording data
tnhnrl 35:2f66ea4863d5 411 //mbedLogger().closeFile();
tnhnrl 32:f2f8ae34aadc 412
tnhnrl 28:16c83a2fdefa 413 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 414 }
tnhnrl 20:8987a9ae2bc7 415
tnhnrl 20:8987a9ae2bc7 416 // what is active?
tnhnrl 32:f2f8ae34aadc 417 pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
tnhnrl 32:f2f8ae34aadc 418
tnhnrl 32:f2f8ae34aadc 419 //record data every 5 seconds
tnhnrl 34:9b66c5188051 420 recordData(_state);
tnhnrl 32:f2f8ae34aadc 421
tnhnrl 16:3363b9f14913 422 break;
tnhnrl 17:7c16b5671d0e 423
tnhnrl 17:7c16b5671d0e 424 case MULTI_DIVE :
tnhnrl 17:7c16b5671d0e 425 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 426 if (!_isTimeoutRunning) {
tnhnrl 17:7c16b5671d0e 427 pc().printf("\r\n\nstate: MULTI-DIVE\r\n");
tnhnrl 17:7c16b5671d0e 428 timer.reset(); // timer goes back to zero
tnhnrl 17:7c16b5671d0e 429 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 430 _isTimeoutRunning = true;
tnhnrl 17:7c16b5671d0e 431
tnhnrl 17:7c16b5671d0e 432 // what needs to be started?
tnhnrl 17:7c16b5671d0e 433 bce().unpause();
tnhnrl 17:7c16b5671d0e 434 batt().unpause();
tnhnrl 17:7c16b5671d0e 435
tnhnrl 21:38c8544db6f4 436 //retrieve commands from structs (loaded from sequence.cfg file)
tnhnrl 32:f2f8ae34aadc 437 float sequence_depth_command = currentStateStruct.depth;
tnhnrl 32:f2f8ae34aadc 438 float sequence_pitch_command = currentStateStruct.pitch;
tnhnrl 17:7c16b5671d0e 439
tnhnrl 17:7c16b5671d0e 440 // what are the commands?
tnhnrl 32:f2f8ae34aadc 441 depthLoop().setCommand(sequence_depth_command);
tnhnrl 32:f2f8ae34aadc 442 pitchLoop().setCommand(sequence_pitch_command);
tnhnrl 21:38c8544db6f4 443 pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 444
tnhnrl 32:f2f8ae34aadc 445 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 446 //createNewFile();
tnhnrl 32:f2f8ae34aadc 447
tnhnrl 32:f2f8ae34aadc 448 //show that this is the start of a new MULTI_DIVE sequence
tnhnrl 32:f2f8ae34aadc 449 recordState(_state);
tnhnrl 32:f2f8ae34aadc 450
tnhnrl 32:f2f8ae34aadc 451 //no max depth recording right now
tnhnrl 32:f2f8ae34aadc 452
tnhnrl 32:f2f8ae34aadc 453 //this was missing
tnhnrl 32:f2f8ae34aadc 454 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 455 recordData(_state);
tnhnrl 17:7c16b5671d0e 456 }
tnhnrl 20:8987a9ae2bc7 457
tnhnrl 17:7c16b5671d0e 458 // how exit?
tnhnrl 17:7c16b5671d0e 459 if (timer > _timeout) {
tnhnrl 21:38c8544db6f4 460 pc().printf("\n\n\rMULTI-DIVE: timed out [time: %0.1f]\n\n\r", timer.read());
tnhnrl 21:38c8544db6f4 461 _state = MULTI_RISE; //new behavior 11/17/2017
tnhnrl 17:7c16b5671d0e 462 timer.reset();
tnhnrl 28:16c83a2fdefa 463 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 464 }
tnhnrl 17:7c16b5671d0e 465 else if (depthLoop().getPosition() > depthLoop().getCommand()) {
tnhnrl 17:7c16b5671d0e 466 pc().printf("MULTI-DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 21:38c8544db6f4 467 _state = MULTI_RISE;
tnhnrl 17:7c16b5671d0e 468 timer.reset();
tnhnrl 28:16c83a2fdefa 469 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 470 }
tnhnrl 20:8987a9ae2bc7 471
tnhnrl 17:7c16b5671d0e 472 // what is active?
tnhnrl 17:7c16b5671d0e 473 pc().printf("MULTI-DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 17:7c16b5671d0e 474 bce().setPosition_mm(depthLoop().getOutput());
tnhnrl 17:7c16b5671d0e 475 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 476
tnhnrl 32:f2f8ae34aadc 477 //record data every 5 seconds
tnhnrl 34:9b66c5188051 478 recordData(_state);
tnhnrl 32:f2f8ae34aadc 479
tnhnrl 17:7c16b5671d0e 480 break;
tnhnrl 17:7c16b5671d0e 481
tnhnrl 17:7c16b5671d0e 482 case MULTI_RISE :
tnhnrl 17:7c16b5671d0e 483 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 484 if (!_isTimeoutRunning) {
tnhnrl 17:7c16b5671d0e 485 pc().printf("\r\n\nstate: MULTI-RISE\r\n");
tnhnrl 17:7c16b5671d0e 486 timer.reset(); // timer goes back to zero
tnhnrl 17:7c16b5671d0e 487 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 488 _isTimeoutRunning = true;
tnhnrl 17:7c16b5671d0e 489
tnhnrl 17:7c16b5671d0e 490 // what needs to be started?
tnhnrl 17:7c16b5671d0e 491 bce().unpause();
tnhnrl 17:7c16b5671d0e 492 batt().unpause();
tnhnrl 17:7c16b5671d0e 493
tnhnrl 17:7c16b5671d0e 494 //NEW: retrieve depth and pitch commands from config file struct
tnhnrl 17:7c16b5671d0e 495 // concept is to load this each time the multi-dive restarts
tnhnrl 17:7c16b5671d0e 496 stateMachine().getDiveSequence();
tnhnrl 17:7c16b5671d0e 497
tnhnrl 17:7c16b5671d0e 498 //retrieve just pitch command from struct
tnhnrl 32:f2f8ae34aadc 499 float sequence_pitch_command = currentStateStruct.pitch;
tnhnrl 17:7c16b5671d0e 500
tnhnrl 17:7c16b5671d0e 501 // what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017
tnhnrl 17:7c16b5671d0e 502 depthLoop().setCommand(0.5);
tnhnrl 32:f2f8ae34aadc 503 pitchLoop().setCommand(-sequence_pitch_command);
tnhnrl 21:38c8544db6f4 504 pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 505
tnhnrl 32:f2f8ae34aadc 506 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 507 //createNewFile();
tnhnrl 32:f2f8ae34aadc 508
tnhnrl 32:f2f8ae34aadc 509 //show that this is the start of a new MULTI_DIVE sequence
tnhnrl 32:f2f8ae34aadc 510 recordState(_state);
tnhnrl 32:f2f8ae34aadc 511
tnhnrl 32:f2f8ae34aadc 512 //this was missing
tnhnrl 32:f2f8ae34aadc 513 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 514 recordData(_state);
tnhnrl 17:7c16b5671d0e 515 }
tnhnrl 20:8987a9ae2bc7 516
tnhnrl 17:7c16b5671d0e 517 // how exit?
tnhnrl 17:7c16b5671d0e 518 if (timer > _timeout) {
tnhnrl 21:38c8544db6f4 519 pc().printf("MULTI-RISE: timed out [time: %0.1f]\n\n\r", timer.read());
tnhnrl 21:38c8544db6f4 520 _state = EMERGENCY_CLIMB;
tnhnrl 17:7c16b5671d0e 521 timer.reset();
tnhnrl 28:16c83a2fdefa 522 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 523
tnhnrl 17:7c16b5671d0e 524 //reset multi-dive sequence to start
tnhnrl 24:c7d9b5bf3829 525 _multi_dive_counter = 0;
tnhnrl 17:7c16b5671d0e 526 }
tnhnrl 20:8987a9ae2bc7 527 else if (depthLoop().getPosition() < 0.5) { // depth is less than 0.5 (zero is surface level)
tnhnrl 17:7c16b5671d0e 528 pc().printf("MULTI-RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 17:7c16b5671d0e 529
tnhnrl 17:7c16b5671d0e 530 //going to next state
tnhnrl 28:16c83a2fdefa 531 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 532
tnhnrl 17:7c16b5671d0e 533 //successful dive-rise sequence CONTINUES the multi-dive sequence
tnhnrl 24:c7d9b5bf3829 534 _multi_dive_counter++;
tnhnrl 17:7c16b5671d0e 535
tnhnrl 17:7c16b5671d0e 536 //UPDATE THE SEQUENCE DATA HERE
tnhnrl 17:7c16b5671d0e 537 stateMachine().getDiveSequence();
tnhnrl 17:7c16b5671d0e 538
tnhnrl 17:7c16b5671d0e 539 //check if this is the end of the dive sequence
tnhnrl 30:2964617e7676 540 //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (FLOAT_BROADCAST)
tnhnrl 30:2964617e7676 541 if (currentStateStruct.state == FLOAT_BROADCAST) {
tnhnrl 28:16c83a2fdefa 542 _state = FLOAT_BROADCAST;
tnhnrl 17:7c16b5671d0e 543 }
tnhnrl 17:7c16b5671d0e 544
tnhnrl 17:7c16b5671d0e 545 else
tnhnrl 21:38c8544db6f4 546 _state = MULTI_DIVE;
tnhnrl 17:7c16b5671d0e 547
tnhnrl 24:c7d9b5bf3829 548 //have to stop this with the _multi_dive_counter variable!
tnhnrl 17:7c16b5671d0e 549 }
tnhnrl 20:8987a9ae2bc7 550
tnhnrl 20:8987a9ae2bc7 551 // what is active?
tnhnrl 17:7c16b5671d0e 552 pc().printf("MULTI-RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 17:7c16b5671d0e 553 bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
tnhnrl 17:7c16b5671d0e 554 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 555
tnhnrl 32:f2f8ae34aadc 556 //record data every 5 seconds
tnhnrl 34:9b66c5188051 557 recordData(_state);
tnhnrl 32:f2f8ae34aadc 558
tnhnrl 17:7c16b5671d0e 559 break;
tnhnrl 32:f2f8ae34aadc 560
tnhnrl 45:16b8162188ca 561 case TRANSMIT_LOG:
tnhnrl 32:f2f8ae34aadc 562 if (!_isTimeoutRunning) {
tnhnrl 45:16b8162188ca 563 pc().printf("\r\n\nstate: TRANSMIT_LOG\r\n");
tnhnrl 32:f2f8ae34aadc 564 timer.reset(); // timer goes back to zero
tnhnrl 32:f2f8ae34aadc 565 timer.start(); // background timer starts running
tnhnrl 32:f2f8ae34aadc 566 _isTimeoutRunning = true;
tnhnrl 32:f2f8ae34aadc 567
tnhnrl 45:16b8162188ca 568 mbedLogger().getNumberOfPacketsInCurrentLog(); //open the file, read the number of lines in the log file
tnhnrl 45:16b8162188ca 569 transmit_packet_number = mbedLogger().getNumberOfPackets();
tnhnrl 45:16b8162188ca 570
tnhnrl 45:16b8162188ca 571 //pc().printf("getNumberOfPacketsInCurrentLog is %d\n\r", transmit_packet_number);
tnhnrl 45:16b8162188ca 572 }
tnhnrl 45:16b8162188ca 573
tnhnrl 45:16b8162188ca 574 if (timer.read() > _timeout) {
tnhnrl 45:16b8162188ca 575 pc().printf("\n\rTRANSMIT_LOG: timed out!\n\r");
tnhnrl 45:16b8162188ca 576 _state = SIT_IDLE;
tnhnrl 45:16b8162188ca 577 timer.reset();
tnhnrl 45:16b8162188ca 578 _isTimeoutRunning = false;
tnhnrl 45:16b8162188ca 579
tnhnrl 45:16b8162188ca 580 mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
tnhnrl 45:16b8162188ca 581 }
tnhnrl 45:16b8162188ca 582
tnhnrl 45:16b8162188ca 583 //IF THIS IS ZERO
tnhnrl 45:16b8162188ca 584 if (mbedLogger().currentPacketNumber() > transmit_packet_number) {
tnhnrl 45:16b8162188ca 585 pc().printf("mbedLogger().currentPacketNumber() > transmit_packet_number");
tnhnrl 45:16b8162188ca 586 _state = SIT_IDLE;
tnhnrl 45:16b8162188ca 587 timer.reset();
tnhnrl 45:16b8162188ca 588 _isTimeoutRunning = false;
tnhnrl 45:16b8162188ca 589
tnhnrl 45:16b8162188ca 590 mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
tnhnrl 32:f2f8ae34aadc 591 }
tnhnrl 45:16b8162188ca 592
tnhnrl 45:16b8162188ca 593 if (mbedLogger().isTransmissionComplete()) {
tnhnrl 45:16b8162188ca 594 pc().printf("StateMachine isTransmissionComplete (true)\n\r");
tnhnrl 45:16b8162188ca 595 _state = SIT_IDLE;
tnhnrl 45:16b8162188ca 596 timer.reset();
tnhnrl 45:16b8162188ca 597 _isTimeoutRunning = false;
tnhnrl 45:16b8162188ca 598
tnhnrl 45:16b8162188ca 599 mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
tnhnrl 45:16b8162188ca 600 }
tnhnrl 45:16b8162188ca 601
tnhnrl 32:f2f8ae34aadc 602
tnhnrl 45:16b8162188ca 603 // what is active? (no hardware should be active)
tnhnrl 45:16b8162188ca 604 mbedLogger().readTransmitPacketOneChar(); //led2 shows you pc readable
tnhnrl 45:16b8162188ca 605
tnhnrl 45:16b8162188ca 606 break;
tnhnrl 45:16b8162188ca 607
tnhnrl 45:16b8162188ca 608 case RECEIVE_SEQUENCE :
tnhnrl 45:16b8162188ca 609 pc().printf("state: RECEIVE_SEQUENCE\n\r");
tnhnrl 45:16b8162188ca 610
tnhnrl 45:16b8162188ca 611 if (!_isTimeoutRunning) {
tnhnrl 45:16b8162188ca 612 pc().printf("RECEIVE_SEQUENCE _isTimeoutRunning\r\n");
tnhnrl 45:16b8162188ca 613 timer.reset(); // timer goes back to zero
tnhnrl 45:16b8162188ca 614 timer.start(); // background timer starts running
tnhnrl 45:16b8162188ca 615 _isTimeoutRunning = true;
tnhnrl 45:16b8162188ca 616 }
tnhnrl 45:16b8162188ca 617
tnhnrl 34:9b66c5188051 618 if (timer.read() > _timeout) {
tnhnrl 45:16b8162188ca 619 pc().printf("RECEIVE_SEQUENCE: timed out!\n\r");
tnhnrl 34:9b66c5188051 620 _state = SIT_IDLE;
tnhnrl 34:9b66c5188051 621 timer.reset();
tnhnrl 32:f2f8ae34aadc 622 _isTimeoutRunning = false;
tnhnrl 32:f2f8ae34aadc 623 }
tnhnrl 32:f2f8ae34aadc 624
tnhnrl 45:16b8162188ca 625 // what is active?
tnhnrl 45:16b8162188ca 626 pc().printf("Receive sequence active?\n\r");
tnhnrl 32:f2f8ae34aadc 627
tnhnrl 32:f2f8ae34aadc 628 break;
tnhnrl 16:3363b9f14913 629
tnhnrl 16:3363b9f14913 630 default :
tnhnrl 17:7c16b5671d0e 631 pc().printf("DEBUG: SIT_IDLE\n\r");
tnhnrl 21:38c8544db6f4 632 _state = SIT_IDLE;
tnhnrl 28:16c83a2fdefa 633 }
tnhnrl 28:16c83a2fdefa 634
tnhnrl 28:16c83a2fdefa 635 //save the state to print to user
tnhnrl 28:16c83a2fdefa 636 if (_previous_state != _state) {
tnhnrl 28:16c83a2fdefa 637 _state_array[_state_array_counter] = _state; //save to state array
tnhnrl 28:16c83a2fdefa 638 _state_array_counter++;
tnhnrl 28:16c83a2fdefa 639
tnhnrl 28:16c83a2fdefa 640 _previous_state = _state;
tnhnrl 28:16c83a2fdefa 641 }
tnhnrl 34:9b66c5188051 642
tnhnrl 34:9b66c5188051 643 return _state;
tnhnrl 34:9b66c5188051 644 // //if the state is SIT_IDLE, return 0 / false (for recording)
tnhnrl 34:9b66c5188051 645 // if (_state == SIT_IDLE)
tnhnrl 34:9b66c5188051 646 // return 0;
tnhnrl 34:9b66c5188051 647 // else
tnhnrl 34:9b66c5188051 648 // return 1; //return true to indicate that you're recording
tnhnrl 16:3363b9f14913 649 }
tnhnrl 20:8987a9ae2bc7 650
tnhnrl 16:3363b9f14913 651 // output the keyboard menu for user's reference
tnhnrl 16:3363b9f14913 652 void StateMachine::showMenu() {
tnhnrl 16:3363b9f14913 653 pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
tnhnrl 16:3363b9f14913 654 pc().printf(" N to find neutral\r\n");
tnhnrl 17:7c16b5671d0e 655 pc().printf(" M to initiate multi-dive cycle\r\n");
tnhnrl 16:3363b9f14913 656 pc().printf(" D to initiate dive cycle\r\n");
tnhnrl 16:3363b9f14913 657 pc().printf(" R to initiate rise\r\n");
tnhnrl 16:3363b9f14913 658 pc().printf(" L to float level\r\n");
tnhnrl 16:3363b9f14913 659 pc().printf(" B to float at broadcast pitch\r\n");
tnhnrl 16:3363b9f14913 660 pc().printf(" E to initiate emergency climb\r\n");
tnhnrl 28:16c83a2fdefa 661 //pc().printf(" H to run homing sequence on both BCE and Batt\r\n");
tnhnrl 16:3363b9f14913 662 pc().printf(" T to tare the depth sensor\r\n");
tnhnrl 28:16c83a2fdefa 663 pc().printf(" Z to show FSM and sub-FSM states.\r\n");
tnhnrl 32:f2f8ae34aadc 664 pc().printf(" P to print the current log file.\r\n");
tnhnrl 32:f2f8ae34aadc 665 pc().printf(" X to print the list of log files.\r\n");
tnhnrl 34:9b66c5188051 666 pc().printf(" V to transmit data (work in progress).\r\n");
tnhnrl 32:f2f8ae34aadc 667 pc().printf("[/] to change bce neutral position: %0.1f\r\n", _neutral_bce_pos_mm);
tnhnrl 32:f2f8ae34aadc 668 pc().printf("</> to change batt neutral position: %0.1f\r\n", _neutral_batt_pos_mm);
tnhnrl 32:f2f8ae34aadc 669 pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitch_command);
tnhnrl 32:f2f8ae34aadc 670 pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depth_command);
tnhnrl 17:7c16b5671d0e 671 pc().printf("+/- to decrease/increase timeout: %d s\r\n",_timeout);
tnhnrl 16:3363b9f14913 672 pc().printf(" 1 BCE PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 673 pc().printf(" 2 BATT PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 674 pc().printf(" 3 Depth PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 675 pc().printf(" 4 Pitch PID sub-menu\r\n");
tnhnrl 28:16c83a2fdefa 676 pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
tnhnrl 16:3363b9f14913 677 pc().printf(" ? to reset mbed\r\n");
tnhnrl 16:3363b9f14913 678 }
tnhnrl 20:8987a9ae2bc7 679
tnhnrl 17:7c16b5671d0e 680 //Find Neutral sub finite state machine
tnhnrl 17:7c16b5671d0e 681 // Note: the sub-fsm only moves the pistons once at the start of each timer loop
tnhnrl 17:7c16b5671d0e 682 // (timer completes, move piston, timer completes, move piston, etc)
tnhnrl 28:16c83a2fdefa 683 int StateMachine::runNeutralStateMachine() {
tnhnrl 24:c7d9b5bf3829 684 switch (_substate) {
tnhnrl 20:8987a9ae2bc7 685 case NEUTRAL_SINKING :
tnhnrl 17:7c16b5671d0e 686 //start the 10 second timer
tnhnrl 28:16c83a2fdefa 687 if (!_isSubStateTimerRunning) {
tnhnrl 24:c7d9b5bf3829 688 _neutral_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds
tnhnrl 17:7c16b5671d0e 689
tnhnrl 39:58375ca6b6ff 690 pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (getSetPosition: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition(), bce().getSetPosition_mm());
tnhnrl 20:8987a9ae2bc7 691
tnhnrl 32:f2f8ae34aadc 692 // what are the commands? (BCE linear actuator active, no pitch movement)
tnhnrl 39:58375ca6b6ff 693 bce().setPosition_mm(bce().getSetPosition_mm() - 2.5); //Troy: There is some strange error where this has to be a hardcoded number.
tnhnrl 23:434f04ef1fad 694
tnhnrl 32:f2f8ae34aadc 695 pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", _neutral_sink_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition());
tnhnrl 17:7c16b5671d0e 696
tnhnrl 28:16c83a2fdefa 697 _isSubStateTimerRunning = true; //disable this block after one iteration
tnhnrl 17:7c16b5671d0e 698 }
tnhnrl 20:8987a9ae2bc7 699
tnhnrl 20:8987a9ae2bc7 700 // how exit?
tnhnrl 20:8987a9ae2bc7 701 //once reached the travel limit, no need to keep trying, so exit
tnhnrl 25:249e4d56b27c 702 if (bce().getPosition_mm() <= 0) {
tnhnrl 25:249e4d56b27c 703 pc().printf("\n\rDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\n\r", bce().getPosition_mm());
tnhnrl 25:249e4d56b27c 704 _substate = NEUTRAL_EXIT;
tnhnrl 28:16c83a2fdefa 705 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 25:249e4d56b27c 706 }
tnhnrl 20:8987a9ae2bc7 707 //once deeper than the commanded setpoint...
tnhnrl 32:f2f8ae34aadc 708 else if (depthLoop().getPosition() > _depth_command) {
tnhnrl 24:c7d9b5bf3829 709 _substate = NEUTRAL_SLOWLY_RISE; // next state
tnhnrl 28:16c83a2fdefa 710 _isSubStateTimerRunning = false; //reset the sub state timer
tnhnrl 20:8987a9ae2bc7 711 }
tnhnrl 20:8987a9ae2bc7 712
tnhnrl 20:8987a9ae2bc7 713 // what is active?
tnhnrl 20:8987a9ae2bc7 714 //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint
tnhnrl 24:c7d9b5bf3829 715 if (timer.read() >= _neutral_timer) {
tnhnrl 32:f2f8ae34aadc 716 pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! [current time: %0.1f]\r\n", timer.read());
tnhnrl 17:7c16b5671d0e 717
tnhnrl 28:16c83a2fdefa 718 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
tnhnrl 17:7c16b5671d0e 719 }
tnhnrl 39:58375ca6b6ff 720
tnhnrl 39:58375ca6b6ff 721 // what is active? (only the buoyancy engine moved every 5 seconds at start)
tnhnrl 39:58375ca6b6ff 722 pc().printf("BCE current pos: %0.1f mm (BCE setpoint: %0.1f mm) (current depth: %0.1f ft)\r", bce().getPosition_mm(),bce().getSetPosition_mm(),depthLoop().getPosition()); //debug
tnhnrl 17:7c16b5671d0e 723 break;
tnhnrl 17:7c16b5671d0e 724
tnhnrl 17:7c16b5671d0e 725 case NEUTRAL_SLOWLY_RISE:
tnhnrl 28:16c83a2fdefa 726 if (!_isSubStateTimerRunning) {
tnhnrl 24:c7d9b5bf3829 727 _neutral_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds
tnhnrl 17:7c16b5671d0e 728
tnhnrl 24:c7d9b5bf3829 729 pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Next extension at %0.1f sec) [current time: %0.1f]\r\n",_neutral_timer,timer.read());
tnhnrl 17:7c16b5671d0e 730
tnhnrl 20:8987a9ae2bc7 731 // what are the commands?
tnhnrl 32:f2f8ae34aadc 732 //move piston at start of sequence (default: extend 2.0 mm)
tnhnrl 39:58375ca6b6ff 733 bce().setPosition_mm(bce().getSetPosition_mm() + 2.0); //no depth command
tnhnrl 23:434f04ef1fad 734
tnhnrl 23:434f04ef1fad 735 // it's okay to run the pitch outer loop now since we've already found pitch level in the previous state
tnhnrl 23:434f04ef1fad 736 pitchLoop().setCommand(0.0);
tnhnrl 24:c7d9b5bf3829 737
tnhnrl 32:f2f8ae34aadc 738 pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", _neutral_rise_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand());
tnhnrl 24:c7d9b5bf3829 739
tnhnrl 28:16c83a2fdefa 740 _isSubStateTimerRunning = true; //disable this block after one iteration
tnhnrl 17:7c16b5671d0e 741 }
tnhnrl 17:7c16b5671d0e 742
tnhnrl 20:8987a9ae2bc7 743 // how exit?
tnhnrl 24:c7d9b5bf3829 744 //once at full travel limit (setPosition) and haven't yet risen, time to give up and exit
tnhnrl 24:c7d9b5bf3829 745 if (bce().getSetPosition_mm() >= bce().getTravelLimit()) {
tnhnrl 24:c7d9b5bf3829 746 _substate = NEUTRAL_EXIT;
tnhnrl 28:16c83a2fdefa 747 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 17:7c16b5671d0e 748 }
tnhnrl 17:7c16b5671d0e 749 //depth rate or sink rate < 0 ft/s, go to the next substate the next iteration
tnhnrl 20:8987a9ae2bc7 750 else if (depthLoop().getVelocity() < 0) { //less than zero ft/s
tnhnrl 24:c7d9b5bf3829 751 pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 ft/s [time: %0.1f]\r\n", timer.read());
tnhnrl 24:c7d9b5bf3829 752 _substate = NEUTRAL_CHECK_PITCH;
tnhnrl 28:16c83a2fdefa 753 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 17:7c16b5671d0e 754 }
tnhnrl 17:7c16b5671d0e 755
tnhnrl 20:8987a9ae2bc7 756 // what is active?
tnhnrl 20:8987a9ae2bc7 757 //once 5 second timer complete, reset the timeout so the state one-shot entry will move the setpoint
tnhnrl 24:c7d9b5bf3829 758 if (timer.read() >= _neutral_timer) {
tnhnrl 32:f2f8ae34aadc 759 pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! [timer: %0.1f]\r\n", timer.read());
tnhnrl 20:8987a9ae2bc7 760
tnhnrl 28:16c83a2fdefa 761 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
tnhnrl 17:7c16b5671d0e 762 }
tnhnrl 23:434f04ef1fad 763
tnhnrl 32:f2f8ae34aadc 764 // what is active? (only the buoyancy engine moved every 5 seconds)
tnhnrl 32:f2f8ae34aadc 765 pc().printf("depthLoop getOutput: %0.1f\r", depthLoop().getOutput()); //debug
tnhnrl 32:f2f8ae34aadc 766 bce().setPosition_mm(depthLoop().getOutput()); // (DID NOT WORK ON BENCH)
tnhnrl 17:7c16b5671d0e 767 break;
tnhnrl 17:7c16b5671d0e 768
danstrider 22:a10ee088403b 769 case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired
danstrider 22:a10ee088403b 770 // start local state timer and init any other one-shot actions
tnhnrl 23:434f04ef1fad 771
tnhnrl 28:16c83a2fdefa 772 if (!_isSubStateTimerRunning) {
tnhnrl 24:c7d9b5bf3829 773 _neutral_timer = timer.read() + 10; // record time when this block is entered and add several seconds
tnhnrl 24:c7d9b5bf3829 774 pc().printf("\r\nNEUTRAL_CHECK_PITCH: Next move in %0.1f sec \r\n",_neutral_timer - timer.read());
danstrider 22:a10ee088403b 775
tnhnrl 32:f2f8ae34aadc 776 // what are the commands? (default: retract or extend 0.5 mm)
tnhnrl 24:c7d9b5bf3829 777 if (pitchLoop().getPosition() > 2) { // nose is high
tnhnrl 39:58375ca6b6ff 778 batt().setPosition_mm(batt().getSetPosition_mm() + 0.5); // move battery forward (using setpoint from linear actuator)
tnhnrl 32:f2f8ae34aadc 779 pc().printf("\n\rNeutral Check Pitch: moving battery FWD in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
danstrider 22:a10ee088403b 780 }
tnhnrl 24:c7d9b5bf3829 781 else if (pitchLoop().getPosition() < -2) { // nose is low
tnhnrl 39:58375ca6b6ff 782 batt().setPosition_mm(batt().getSetPosition_mm() - 0.5); // move battery aft (using setpoint from linear actuator)
tnhnrl 32:f2f8ae34aadc 783 pc().printf("\n\rNeutral Check Pitch: moving battery AFT in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
danstrider 22:a10ee088403b 784 }
tnhnrl 24:c7d9b5bf3829 785
tnhnrl 28:16c83a2fdefa 786 _isSubStateTimerRunning = true; //disable this block after one iteration
danstrider 22:a10ee088403b 787 }
tnhnrl 20:8987a9ae2bc7 788
tnhnrl 28:16c83a2fdefa 789 // how exit?
tnhnrl 20:8987a9ae2bc7 790 //pitch angle and pitch rate within small tolerance
tnhnrl 20:8987a9ae2bc7 791 //benchtop tests confirm angle needs to be around 2 degrees
tnhnrl 23:434f04ef1fad 792 if ((fabs(pitchLoop().getPosition()) < 2.0) and (fabs(pitchLoop().getVelocity()) < 5.0)) {
tnhnrl 24:c7d9b5bf3829 793 pc().printf("Debug: Found Level (NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH)\n\r"); //debug
danstrider 22:a10ee088403b 794 // found level, but don't need to save anything this time
tnhnrl 23:434f04ef1fad 795
tnhnrl 28:16c83a2fdefa 796 if (depthLoop().getPosition() > _max_recorded_depth_neutral) { //debug
tnhnrl 28:16c83a2fdefa 797 _max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded
tnhnrl 28:16c83a2fdefa 798 }
tnhnrl 28:16c83a2fdefa 799
tnhnrl 23:434f04ef1fad 800 // found level and at depth too, so save it all now
tnhnrl 32:f2f8ae34aadc 801 if (_substate == NEUTRAL_CHECK_PITCH) {
danstrider 22:a10ee088403b 802 //save positions locally
danstrider 22:a10ee088403b 803 _neutral_batt_pos_mm = batt().getPosition_mm();
danstrider 22:a10ee088403b 804 _neutral_bce_pos_mm = bce().getPosition_mm();
danstrider 22:a10ee088403b 805
danstrider 22:a10ee088403b 806 //set the neutral positions in each outer loop
danstrider 22:a10ee088403b 807 depthLoop().setOutputOffset(_neutral_bce_pos_mm);
danstrider 22:a10ee088403b 808 pitchLoop().setOutputOffset(_neutral_batt_pos_mm);
danstrider 22:a10ee088403b 809
danstrider 22:a10ee088403b 810 // save into the depth.txt and pitch.txt files
danstrider 22:a10ee088403b 811 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset
danstrider 22:a10ee088403b 812 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset
danstrider 22:a10ee088403b 813
tnhnrl 32:f2f8ae34aadc 814 pc().printf("\n\n\r>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
danstrider 22:a10ee088403b 815
tnhnrl 24:c7d9b5bf3829 816 _substate = NEUTRAL_EXIT;
tnhnrl 28:16c83a2fdefa 817 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
tnhnrl 24:c7d9b5bf3829 818 }
tnhnrl 24:c7d9b5bf3829 819
tnhnrl 24:c7d9b5bf3829 820 else {
tnhnrl 24:c7d9b5bf3829 821 pc().printf("\n\rDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\n\r");
tnhnrl 24:c7d9b5bf3829 822 _substate = NEUTRAL_EXIT;
danstrider 22:a10ee088403b 823 }
tnhnrl 17:7c16b5671d0e 824 }
danstrider 22:a10ee088403b 825
danstrider 22:a10ee088403b 826 // what is active?
danstrider 22:a10ee088403b 827 //once timer complete, reset the timeout so the state one-shot entry will move the setpoint
tnhnrl 24:c7d9b5bf3829 828 if (timer.read() >= _neutral_timer) {
danstrider 22:a10ee088403b 829 pc().printf("\r\n\nlevel timer COMPLETE!");
danstrider 22:a10ee088403b 830 pc().printf("\r\n\n (BATT POS: %0.1f) moving 1 mm [timer: %0.1f]\r\n", batt().getPosition_mm(), timer.read());
tnhnrl 28:16c83a2fdefa 831 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
danstrider 22:a10ee088403b 832 }
tnhnrl 17:7c16b5671d0e 833 break;
danstrider 22:a10ee088403b 834
danstrider 22:a10ee088403b 835 //this state could be removed, it is only used as a transition but is needed to stop entering this function
danstrider 22:a10ee088403b 836 case NEUTRAL_EXIT :
tnhnrl 23:434f04ef1fad 837 pc().printf("substate: NEUTRAL_EXIT\n\r");
tnhnrl 20:8987a9ae2bc7 838 break;
tnhnrl 21:38c8544db6f4 839
danstrider 22:a10ee088403b 840 default :
tnhnrl 24:c7d9b5bf3829 841 pc().printf("how did we get to substate: default?\n\r"); //debug
tnhnrl 23:434f04ef1fad 842 //a default within the sub-state machine
tnhnrl 24:c7d9b5bf3829 843 _substate = NEUTRAL_EXIT;
danstrider 22:a10ee088403b 844 break;
tnhnrl 17:7c16b5671d0e 845 }
tnhnrl 20:8987a9ae2bc7 846
tnhnrl 30:2964617e7676 847 // reset the sub-FSM if needed (useful if you need to redo the neutral-finding sequence)
tnhnrl 24:c7d9b5bf3829 848 if (_substate == NEUTRAL_EXIT) {
tnhnrl 24:c7d9b5bf3829 849 pc().printf("******************************** EXITING sub-FSM! *******************************\n\n\r");
tnhnrl 24:c7d9b5bf3829 850
tnhnrl 30:2964617e7676 851 //reset internal sub-state back to first entry conditions (first state is immediately sinking)
tnhnrl 30:2964617e7676 852 _substate = NEUTRAL_SINKING;
tnhnrl 28:16c83a2fdefa 853 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 21:38c8544db6f4 854
tnhnrl 24:c7d9b5bf3829 855 //record sub-states to view after sequence
tnhnrl 30:2964617e7676 856 _substate_array[_substate_array_counter] = NEUTRAL_EXIT; //save exit to state array
tnhnrl 28:16c83a2fdefa 857 _substate_array_counter++;
tnhnrl 23:434f04ef1fad 858
tnhnrl 24:c7d9b5bf3829 859 //reset _previous_substate on exit (has to be done in FIND_NEUTRAL if emergency exit)
tnhnrl 24:c7d9b5bf3829 860 _previous_substate = -1;
tnhnrl 24:c7d9b5bf3829 861
tnhnrl 24:c7d9b5bf3829 862 //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed
tnhnrl 24:c7d9b5bf3829 863 return NEUTRAL_EXIT; // message to calling function we just exited
tnhnrl 21:38c8544db6f4 864 }
tnhnrl 23:434f04ef1fad 865 else {
tnhnrl 24:c7d9b5bf3829 866 //record sub-states to view after sequence (when changed)
tnhnrl 24:c7d9b5bf3829 867 if (_previous_substate != _substate) {
tnhnrl 28:16c83a2fdefa 868 _substate_array[_substate_array_counter] = _substate; //save current state to state array
tnhnrl 28:16c83a2fdefa 869 _substate_array_counter++;
tnhnrl 24:c7d9b5bf3829 870
tnhnrl 24:c7d9b5bf3829 871 //record the current substate for comparison
tnhnrl 24:c7d9b5bf3829 872 _previous_substate = _substate;
tnhnrl 24:c7d9b5bf3829 873 }
tnhnrl 24:c7d9b5bf3829 874
tnhnrl 24:c7d9b5bf3829 875 return _substate; // message to calling function of what sub-state it's in
tnhnrl 23:434f04ef1fad 876 }
tnhnrl 17:7c16b5671d0e 877 }
tnhnrl 20:8987a9ae2bc7 878
tnhnrl 20:8987a9ae2bc7 879 // keyboard runs independently of the state machine, handling one key at a time
tnhnrl 20:8987a9ae2bc7 880 //keyboard updates the desired _keyboard_state that is used in the state machine
tnhnrl 20:8987a9ae2bc7 881 //and only allows input when the state is "idle"
tnhnrl 17:7c16b5671d0e 882 void StateMachine::keyboard() {
tnhnrl 16:3363b9f14913 883 char userInput;
tnhnrl 20:8987a9ae2bc7 884
tnhnrl 16:3363b9f14913 885 // check keyboard and make settings changes as requested
tnhnrl 17:7c16b5671d0e 886 // states can be changed only at the start of a sequence (when the system is in SIT_IDLE)
tnhnrl 21:38c8544db6f4 887
tnhnrl 37:357e98a929cc 888 //TEST
tnhnrl 37:357e98a929cc 889 int _keyboard_state = 0; //made this a local variable because it was retaining the last keyboard state
tnhnrl 21:38c8544db6f4 890
tnhnrl 28:16c83a2fdefa 891 if (pc().readable() && (_state == SIT_IDLE || _state == KEYBOARD)) {
tnhnrl 16:3363b9f14913 892 // get the key
tnhnrl 17:7c16b5671d0e 893 userInput = pc().getc();
tnhnrl 17:7c16b5671d0e 894
tnhnrl 28:16c83a2fdefa 895 //record that the keyboard was used
tnhnrl 28:16c83a2fdefa 896 _state_array[_state_array_counter] = KEYBOARD;
tnhnrl 28:16c83a2fdefa 897 _state_array_counter++;
tnhnrl 28:16c83a2fdefa 898
tnhnrl 21:38c8544db6f4 899 // keyboard has to reset timer each time it's used
tnhnrl 28:16c83a2fdefa 900 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 901
tnhnrl 16:3363b9f14913 902 // check command against desired control buttons
tnhnrl 16:3363b9f14913 903 if (userInput == 'D' or userInput == 'd') {
tnhnrl 17:7c16b5671d0e 904 _keyboard_state = DIVE;
tnhnrl 16:3363b9f14913 905 }
tnhnrl 16:3363b9f14913 906 else if (userInput == 'N' or userInput == 'n') {
tnhnrl 17:7c16b5671d0e 907 _keyboard_state = FIND_NEUTRAL;
tnhnrl 17:7c16b5671d0e 908 }
tnhnrl 17:7c16b5671d0e 909 else if (userInput == 'M' or userInput == 'm') {
tnhnrl 17:7c16b5671d0e 910 //currently does not run if there is no file.
tnhnrl 17:7c16b5671d0e 911
tnhnrl 17:7c16b5671d0e 912 //need to add method to Sequence Controller that returns -1
tnhnrl 17:7c16b5671d0e 913 // or some check that insures you cannot run the dive sequence without a file
tnhnrl 17:7c16b5671d0e 914
tnhnrl 17:7c16b5671d0e 915 stateMachine().getDiveSequence(); //get first sequence on keyboard press
tnhnrl 17:7c16b5671d0e 916 _keyboard_state = currentStateStruct.state;
tnhnrl 17:7c16b5671d0e 917
tnhnrl 17:7c16b5671d0e 918 pc().printf("Starting Dive Sequence Controller! (state: %d)\n\r", _keyboard_state); //neutral sequence and dive cycles
tnhnrl 16:3363b9f14913 919 }
tnhnrl 45:16b8162188ca 920 else if (userInput == 'R') {
tnhnrl 17:7c16b5671d0e 921 _keyboard_state = RISE;
tnhnrl 16:3363b9f14913 922 }
tnhnrl 45:16b8162188ca 923 else if (userInput == 'L') {
tnhnrl 17:7c16b5671d0e 924 _keyboard_state = FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 925 }
tnhnrl 45:16b8162188ca 926 else if (userInput == 'B') {
tnhnrl 17:7c16b5671d0e 927 _keyboard_state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 928 }
tnhnrl 45:16b8162188ca 929 else if (userInput == 'E') {
tnhnrl 17:7c16b5671d0e 930 _keyboard_state = EMERGENCY_CLIMB;
tnhnrl 16:3363b9f14913 931 }
tnhnrl 32:f2f8ae34aadc 932 else if (userInput == 'P') {
tnhnrl 32:f2f8ae34aadc 933 //Print current SD card log file
tnhnrl 36:966a86937e17 934 //printCurrentSdLog();
tnhnrl 36:966a86937e17 935 mbedLogger().printCurrentLogFile(); //print the current log file to the screen
tnhnrl 32:f2f8ae34aadc 936 }
tnhnrl 32:f2f8ae34aadc 937 else if (userInput == 'X') {
tnhnrl 45:16b8162188ca 938 mbedLogger().printMbedDirectory(); //print all log files to the screen
tnhnrl 32:f2f8ae34aadc 939 }
tnhnrl 45:16b8162188ca 940 else if (userInput == 'O') {
tnhnrl 45:16b8162188ca 941 _keyboard_state = TRANSMIT_LOG; //Transmit data (work in progress)
tnhnrl 45:16b8162188ca 942 }
tnhnrl 45:16b8162188ca 943 else if (userInput == 'Z') {
tnhnrl 28:16c83a2fdefa 944 pc().printf("FSG FSM States: \n\r");
tnhnrl 28:16c83a2fdefa 945 string string_state;
tnhnrl 28:16c83a2fdefa 946
tnhnrl 28:16c83a2fdefa 947 for (int i = 0; i < _state_array_counter; i++) {
tnhnrl 28:16c83a2fdefa 948 if (_state_array[i] == SIT_IDLE)
tnhnrl 28:16c83a2fdefa 949 string_state = "SIT_IDLE <END>";
tnhnrl 28:16c83a2fdefa 950 else if (_state_array[i] == FIND_NEUTRAL)
tnhnrl 28:16c83a2fdefa 951 string_state = "FIND_NEUTRAL";
tnhnrl 28:16c83a2fdefa 952 else if (_state_array[i] == DIVE)
tnhnrl 28:16c83a2fdefa 953 string_state = "DIVE";
tnhnrl 28:16c83a2fdefa 954 else if (_state_array[i] == RISE)
tnhnrl 28:16c83a2fdefa 955 string_state = "RISE";
tnhnrl 28:16c83a2fdefa 956 else if (_state_array[i] == FLOAT_LEVEL)
tnhnrl 28:16c83a2fdefa 957 string_state = "FLOAT_LEVEL";
tnhnrl 28:16c83a2fdefa 958 else if (_state_array[i] == FLOAT_BROADCAST)
tnhnrl 28:16c83a2fdefa 959 string_state = "FLOAT_BROADCAST";
tnhnrl 28:16c83a2fdefa 960 else if (_state_array[i] == EMERGENCY_CLIMB)
tnhnrl 28:16c83a2fdefa 961 string_state = "EMERGENCY_CLIMB";
tnhnrl 28:16c83a2fdefa 962 else if (_state_array[i] == MULTI_DIVE)
tnhnrl 28:16c83a2fdefa 963 string_state = "MULTI_DIVE";
tnhnrl 28:16c83a2fdefa 964 else if (_state_array[i] == MULTI_RISE)
tnhnrl 28:16c83a2fdefa 965 string_state = "MULTI_RISE";
tnhnrl 28:16c83a2fdefa 966 else if (_state_array[i] == KEYBOARD)
tnhnrl 28:16c83a2fdefa 967 string_state = "KEYBOARD";
tnhnrl 28:16c83a2fdefa 968 pc().printf("State #%d: %d (%s)\n\r", i, _state_array[i], string_state.c_str());
tnhnrl 28:16c83a2fdefa 969 }
tnhnrl 28:16c83a2fdefa 970
tnhnrl 28:16c83a2fdefa 971 pc().printf("\n\rNeutral sub-FSM States: \n\r");
tnhnrl 28:16c83a2fdefa 972 string string_substate;
tnhnrl 28:16c83a2fdefa 973
tnhnrl 28:16c83a2fdefa 974 for (int i = 0; i < _substate_array_counter; i++) {
tnhnrl 32:f2f8ae34aadc 975 if (_substate_array[i] == NEUTRAL_SINKING)
tnhnrl 28:16c83a2fdefa 976 string_substate = "NEUTRAL_SINKING";
tnhnrl 28:16c83a2fdefa 977 else if (_substate_array[i] == NEUTRAL_SLOWLY_RISE)
tnhnrl 28:16c83a2fdefa 978 string_substate = "NEUTRAL_SLOWLY_RISE";
tnhnrl 28:16c83a2fdefa 979 else if (_substate_array[i] == NEUTRAL_CHECK_PITCH)
tnhnrl 28:16c83a2fdefa 980 string_substate = "NEUTRAL_CHECK_PITCH";
tnhnrl 28:16c83a2fdefa 981 else if (_substate_array[i] == NEUTRAL_EXIT)
tnhnrl 28:16c83a2fdefa 982 string_substate = "NEUTRAL_EXIT <-- ";
tnhnrl 28:16c83a2fdefa 983 else if (_substate_array[i] == EMERGENCY_CLIMB)
tnhnrl 28:16c83a2fdefa 984 string_substate = " -- > EMERGENCY_CLIMB <-- ";
tnhnrl 28:16c83a2fdefa 985 pc().printf("Neutral Substate #%d: %d (%s)\n\r", i, _state_array[i], string_substate.c_str());
tnhnrl 28:16c83a2fdefa 986 }
tnhnrl 28:16c83a2fdefa 987 pc().printf("\n\r"); //make space between printouts
tnhnrl 16:3363b9f14913 988 }
tnhnrl 16:3363b9f14913 989 else if (userInput == 'T' or userInput == 't') {
tnhnrl 16:3363b9f14913 990 pc().printf("taring depth sensor\r\n");
tnhnrl 16:3363b9f14913 991 pc().printf("Pre-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
tnhnrl 16:3363b9f14913 992 wait(0.1);
tnhnrl 16:3363b9f14913 993 depth().tare(); // tares to ambient (do on surface)
tnhnrl 16:3363b9f14913 994 pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
tnhnrl 16:3363b9f14913 995 }
tnhnrl 16:3363b9f14913 996
tnhnrl 16:3363b9f14913 997 else if (userInput == '[' or userInput == '{') {
tnhnrl 32:f2f8ae34aadc 998 _neutral_bce_pos_mm = depthLoop().getOutputOffset() - 1;
tnhnrl 32:f2f8ae34aadc 999 depthLoop().setOutputOffset(_neutral_bce_pos_mm); // decrease the bce neutral setpoint
tnhnrl 32:f2f8ae34aadc 1000 pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 32:f2f8ae34aadc 1001 // save neutral depth value to config file
tnhnrl 32:f2f8ae34aadc 1002 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
tnhnrl 16:3363b9f14913 1003 }
tnhnrl 16:3363b9f14913 1004 else if (userInput == ']' or userInput == '}') {
tnhnrl 32:f2f8ae34aadc 1005 _neutral_bce_pos_mm = depthLoop().getOutputOffset() + 1;
tnhnrl 32:f2f8ae34aadc 1006 depthLoop().setOutputOffset(_neutral_bce_pos_mm); // increase the bce neutral setpoint
tnhnrl 32:f2f8ae34aadc 1007 pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 32:f2f8ae34aadc 1008 // save neutral depth value to config file
tnhnrl 32:f2f8ae34aadc 1009 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
tnhnrl 16:3363b9f14913 1010 }
tnhnrl 16:3363b9f14913 1011 else if (userInput == '<' or userInput == ',') {
tnhnrl 32:f2f8ae34aadc 1012 _neutral_batt_pos_mm = pitchLoop().getOutputOffset() - 1;
tnhnrl 32:f2f8ae34aadc 1013 pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // decrease the batt neutral setpoint
tnhnrl 32:f2f8ae34aadc 1014 pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 32:f2f8ae34aadc 1015 // save neutral pitch value to config file
tnhnrl 32:f2f8ae34aadc 1016 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
tnhnrl 16:3363b9f14913 1017 }
tnhnrl 16:3363b9f14913 1018 else if (userInput == '>' or userInput == '.') {
tnhnrl 32:f2f8ae34aadc 1019 _neutral_batt_pos_mm = pitchLoop().getOutputOffset() + 1;
tnhnrl 32:f2f8ae34aadc 1020 pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // increase the batt neutral setpoint
tnhnrl 32:f2f8ae34aadc 1021 pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 32:f2f8ae34aadc 1022 // save neutral pitch value to config file
tnhnrl 32:f2f8ae34aadc 1023 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
tnhnrl 16:3363b9f14913 1024 }
tnhnrl 16:3363b9f14913 1025
tnhnrl 16:3363b9f14913 1026 else if (userInput == '?') {
tnhnrl 16:3363b9f14913 1027 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
tnhnrl 16:3363b9f14913 1028 wait(0.5);
tnhnrl 16:3363b9f14913 1029 mbed_reset();
tnhnrl 16:3363b9f14913 1030 }
tnhnrl 20:8987a9ae2bc7 1031
tnhnrl 16:3363b9f14913 1032 // change settings
tnhnrl 16:3363b9f14913 1033 else if (userInput == 'Q' or userInput == 'q') {
tnhnrl 32:f2f8ae34aadc 1034 _pitch_command -= 0.5; //decrement the pitch setpoint
tnhnrl 32:f2f8ae34aadc 1035 pitchLoop().setCommand(_pitch_command);
tnhnrl 16:3363b9f14913 1036 pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 1037 }
tnhnrl 16:3363b9f14913 1038 else if (userInput == 'W' or userInput == 'w') {
tnhnrl 32:f2f8ae34aadc 1039 _pitch_command += 0.5; //increment the pitch setpoint
tnhnrl 32:f2f8ae34aadc 1040 pitchLoop().setCommand(_pitch_command);
tnhnrl 16:3363b9f14913 1041 pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 1042 }
tnhnrl 16:3363b9f14913 1043 else if (userInput == 'A' or userInput == 'a') {
tnhnrl 32:f2f8ae34aadc 1044 _depth_command -= 0.5; //decrement the depth setpoint
tnhnrl 32:f2f8ae34aadc 1045 depthLoop().setCommand(_depth_command);
tnhnrl 16:3363b9f14913 1046 pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
tnhnrl 16:3363b9f14913 1047 }
tnhnrl 16:3363b9f14913 1048 else if (userInput == 'S' or userInput == 's') {
tnhnrl 32:f2f8ae34aadc 1049 _depth_command += 0.5; //increment the depth setpoint
tnhnrl 32:f2f8ae34aadc 1050 depthLoop().setCommand(_depth_command);
tnhnrl 16:3363b9f14913 1051 pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
tnhnrl 16:3363b9f14913 1052 }
tnhnrl 16:3363b9f14913 1053 else if (userInput == '-') {
tnhnrl 17:7c16b5671d0e 1054 _timeout -= 10.0; //decrement the timeout
tnhnrl 17:7c16b5671d0e 1055 pc().printf(">>> timeout decreased: %d\r\n", _timeout);
tnhnrl 16:3363b9f14913 1056 }
tnhnrl 16:3363b9f14913 1057 else if (userInput == '=' or userInput == '+') {
tnhnrl 17:7c16b5671d0e 1058 _timeout += 10.0; //increment the timeout
tnhnrl 17:7c16b5671d0e 1059 pc().printf(">>> timeout increased: %d\r\n", _timeout);
tnhnrl 16:3363b9f14913 1060 }
tnhnrl 16:3363b9f14913 1061
tnhnrl 16:3363b9f14913 1062 // go to sub-menus for the PID gains (this is blocking)
tnhnrl 16:3363b9f14913 1063 else if (userInput == '1') {
tnhnrl 16:3363b9f14913 1064 keyboard_menu_BCE_PID_settings();
tnhnrl 16:3363b9f14913 1065 }
tnhnrl 16:3363b9f14913 1066 else if (userInput == '2') {
tnhnrl 16:3363b9f14913 1067 keyboard_menu_BATT_PID_settings();
tnhnrl 16:3363b9f14913 1068 }
tnhnrl 16:3363b9f14913 1069 else if (userInput == '3') {
tnhnrl 16:3363b9f14913 1070 keyboard_menu_DEPTH_PID_settings();
tnhnrl 16:3363b9f14913 1071 }
tnhnrl 16:3363b9f14913 1072 else if (userInput == '4') {
tnhnrl 16:3363b9f14913 1073 keyboard_menu_PITCH_PID_settings();
tnhnrl 16:3363b9f14913 1074 }
tnhnrl 16:3363b9f14913 1075
tnhnrl 16:3363b9f14913 1076 else if (userInput == 'C' or userInput == 'c') {
tnhnrl 32:f2f8ae34aadc 1077
tnhnrl 32:f2f8ae34aadc 1078 pc().printf("\n\n\rCURRENT STATUS AND PARAMETERS:\n\r");
tnhnrl 32:f2f8ae34aadc 1079 pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
tnhnrl 32:f2f8ae34aadc 1080 pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
tnhnrl 16:3363b9f14913 1081 pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
tnhnrl 16:3363b9f14913 1082 pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
tnhnrl 16:3363b9f14913 1083 pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
tnhnrl 16:3363b9f14913 1084 pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
tnhnrl 16:3363b9f14913 1085 pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
tnhnrl 16:3363b9f14913 1086 pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 1087
tnhnrl 32:f2f8ae34aadc 1088 pc().printf("\n\rNeutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
tnhnrl 28:16c83a2fdefa 1089 pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 28:16c83a2fdefa 1090 pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 32:f2f8ae34aadc 1091 pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
tnhnrl 38:83d06c294807 1092
tnhnrl 38:83d06c294807 1093 pc().printf("\n\r");
tnhnrl 38:83d06c294807 1094 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 1095 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 38:83d06c294807 1096 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 38:83d06c294807 1097 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());
tnhnrl 16:3363b9f14913 1098 }
tnhnrl 17:7c16b5671d0e 1099
tnhnrl 17:7c16b5671d0e 1100 //when you read the keyboard successfully, change the state
tnhnrl 28:16c83a2fdefa 1101 _state = _keyboard_state; //set state at the end of this function
tnhnrl 38:83d06c294807 1102 //pc().printf("\n\n\r ********* KEYBOARD STATE: %d *********\n\n\r", _state);
tnhnrl 16:3363b9f14913 1103 }
tnhnrl 16:3363b9f14913 1104 }
tnhnrl 20:8987a9ae2bc7 1105
tnhnrl 16:3363b9f14913 1106 void StateMachine::keyboard_menu_BCE_PID_settings() {
tnhnrl 16:3363b9f14913 1107 char PID_key;
tnhnrl 16:3363b9f14913 1108 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 38:83d06c294807 1109 float BCE_KP = bce().getControllerP(); // load current value
tnhnrl 38:83d06c294807 1110 float BCE_KI = bce().getControllerI(); // load current global value
tnhnrl 38:83d06c294807 1111 float BCE_KD = bce().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 1112
tnhnrl 16:3363b9f14913 1113 // show the menu
tnhnrl 16:3363b9f14913 1114 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 1115 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 1116 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
tnhnrl 21:38c8544db6f4 1117 pc().printf("bce P: %3.2f, I: %3.2f, D %3.2f, zero %d, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
tnhnrl 16:3363b9f14913 1118
tnhnrl 16:3363b9f14913 1119 // handle the key presses
tnhnrl 16:3363b9f14913 1120 while(1) {
tnhnrl 16:3363b9f14913 1121 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 1122 if (pc().readable()) {
tnhnrl 16:3363b9f14913 1123 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 1124 }
tnhnrl 16:3363b9f14913 1125 else {
tnhnrl 16:3363b9f14913 1126 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 1127 }
tnhnrl 16:3363b9f14913 1128
tnhnrl 16:3363b9f14913 1129 // handle the user's key input
tnhnrl 16:3363b9f14913 1130 if (PID_key == '-') {
tnhnrl 38:83d06c294807 1131 BCE_KP -= gain_step_size;
tnhnrl 38:83d06c294807 1132 pc().printf("P gain: %0.5f \r\n", BCE_KP);
tnhnrl 16:3363b9f14913 1133 }
tnhnrl 16:3363b9f14913 1134 else if (PID_key == '=') {
tnhnrl 38:83d06c294807 1135 BCE_KP += gain_step_size;
tnhnrl 38:83d06c294807 1136 pc().printf("P gain: %0.5f \r\n", BCE_KP);
tnhnrl 16:3363b9f14913 1137 }
tnhnrl 16:3363b9f14913 1138 else if (PID_key == '[') {
tnhnrl 38:83d06c294807 1139 BCE_KI -= gain_step_size;
tnhnrl 38:83d06c294807 1140 pc().printf("I gain: %0.5f \r\n", BCE_KI);
tnhnrl 16:3363b9f14913 1141 }
tnhnrl 16:3363b9f14913 1142 else if (PID_key == ']') {
tnhnrl 38:83d06c294807 1143 BCE_KI += gain_step_size;
tnhnrl 38:83d06c294807 1144 pc().printf("I gain: %0.5f \r\n", BCE_KI);
tnhnrl 16:3363b9f14913 1145 }
tnhnrl 16:3363b9f14913 1146 else if (PID_key == ';') {
tnhnrl 38:83d06c294807 1147 BCE_KD -= gain_step_size;
tnhnrl 38:83d06c294807 1148 pc().printf("D gain: %0.5f \r\n", BCE_KD);
tnhnrl 16:3363b9f14913 1149 }
tnhnrl 16:3363b9f14913 1150 else if (PID_key == '\'') {
tnhnrl 38:83d06c294807 1151 BCE_KD += gain_step_size;
tnhnrl 38:83d06c294807 1152 pc().printf("D gain: %0.5f \r\n", BCE_KD);
tnhnrl 16:3363b9f14913 1153 }
tnhnrl 16:3363b9f14913 1154 else if (PID_key == 'S') { // user wants to save these modified values
tnhnrl 16:3363b9f14913 1155 // set values
tnhnrl 38:83d06c294807 1156 bce().setControllerP(BCE_KP);
tnhnrl 38:83d06c294807 1157 bce().setControllerI(BCE_KI);
tnhnrl 38:83d06c294807 1158 bce().setControllerD(BCE_KD);
tnhnrl 16:3363b9f14913 1159
tnhnrl 38:83d06c294807 1160 // save to "BATT.TXT" file
tnhnrl 38:83d06c294807 1161 configFileIO().saveBCEData(BCE_KP, BCE_KI, BCE_KD);
tnhnrl 38:83d06c294807 1162
tnhnrl 16:3363b9f14913 1163 break; //exit the while loop
tnhnrl 16:3363b9f14913 1164 }
tnhnrl 16:3363b9f14913 1165 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 1166 break; //exit the while loop
tnhnrl 16:3363b9f14913 1167 }
tnhnrl 16:3363b9f14913 1168 else {
tnhnrl 16:3363b9f14913 1169 pc().printf("\n\rThis key does nothing here. ");
tnhnrl 16:3363b9f14913 1170 }
tnhnrl 16:3363b9f14913 1171 }
tnhnrl 16:3363b9f14913 1172 }
tnhnrl 20:8987a9ae2bc7 1173
tnhnrl 16:3363b9f14913 1174 void StateMachine::keyboard_menu_BATT_PID_settings() {
tnhnrl 16:3363b9f14913 1175 char PID_key;
tnhnrl 16:3363b9f14913 1176 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 38:83d06c294807 1177 float batt_KP = batt().getControllerP(); // load current global value
tnhnrl 38:83d06c294807 1178 float batt_KI = batt().getControllerI(); // load current global value
tnhnrl 38:83d06c294807 1179 float batt_KD = batt().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 1180
tnhnrl 16:3363b9f14913 1181 // print the menu
tnhnrl 16:3363b9f14913 1182 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 1183 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 1184 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
tnhnrl 21:38c8544db6f4 1185 pc().printf("batt P: %3.2f, I: %3.2f, D %3.2f, zero %d, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
tnhnrl 20:8987a9ae2bc7 1186
tnhnrl 16:3363b9f14913 1187 // handle the key presses
tnhnrl 16:3363b9f14913 1188 while(1) {
tnhnrl 16:3363b9f14913 1189 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 1190 if (pc().readable()) {
tnhnrl 16:3363b9f14913 1191 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 1192 }
tnhnrl 16:3363b9f14913 1193 else {
tnhnrl 16:3363b9f14913 1194 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 1195 }
tnhnrl 16:3363b9f14913 1196
tnhnrl 16:3363b9f14913 1197 // handle the user's key input
tnhnrl 16:3363b9f14913 1198 if (PID_key == '-') {
tnhnrl 38:83d06c294807 1199 batt_KP -= gain_step_size;
tnhnrl 38:83d06c294807 1200 pc().printf("\rP gain: %0.5f ", batt_KP);
tnhnrl 16:3363b9f14913 1201 }
tnhnrl 16:3363b9f14913 1202 else if (PID_key == '=') {
tnhnrl 38:83d06c294807 1203 batt_KP += gain_step_size;
tnhnrl 38:83d06c294807 1204 pc().printf("\rP gain: %0.5f ", batt_KP);
tnhnrl 16:3363b9f14913 1205 }
tnhnrl 16:3363b9f14913 1206 else if (PID_key == '[') {
tnhnrl 38:83d06c294807 1207 batt_KI -= gain_step_size;
tnhnrl 38:83d06c294807 1208 pc().printf("\rI gain: %0.5f ", batt_KI);
tnhnrl 16:3363b9f14913 1209 }
tnhnrl 16:3363b9f14913 1210 else if (PID_key == ']') {
tnhnrl 38:83d06c294807 1211 batt_KI += gain_step_size;
tnhnrl 38:83d06c294807 1212 pc().printf("\rI gain: %0.5f ", batt_KI);
tnhnrl 16:3363b9f14913 1213 }
tnhnrl 16:3363b9f14913 1214 else if (PID_key == ';') {
tnhnrl 38:83d06c294807 1215 batt_KD -= gain_step_size;
tnhnrl 38:83d06c294807 1216 pc().printf("\rD gain: %0.5f ", batt_KD);
tnhnrl 16:3363b9f14913 1217 }
tnhnrl 16:3363b9f14913 1218 else if (PID_key == '\'') {
tnhnrl 38:83d06c294807 1219 batt_KD += gain_step_size;
tnhnrl 38:83d06c294807 1220 pc().printf("\rD gain: %0.5f ", batt_KD);
tnhnrl 16:3363b9f14913 1221 }
tnhnrl 16:3363b9f14913 1222 else if (PID_key == 'S') { // user wants to save the modified values
tnhnrl 16:3363b9f14913 1223 // set global values
tnhnrl 38:83d06c294807 1224 batt().setControllerP(batt_KP);
tnhnrl 38:83d06c294807 1225 batt().setControllerI(batt_KI);
tnhnrl 38:83d06c294807 1226 batt().setControllerD(batt_KD);
tnhnrl 16:3363b9f14913 1227
tnhnrl 38:83d06c294807 1228 // save to "BATT.TXT" file
tnhnrl 38:83d06c294807 1229 configFileIO().saveBattData(batt_KP, batt_KI, batt_KD);
tnhnrl 38:83d06c294807 1230
tnhnrl 16:3363b9f14913 1231 break; //exit the while loop
tnhnrl 16:3363b9f14913 1232 }
tnhnrl 16:3363b9f14913 1233 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 1234 break; //exit the while loop
tnhnrl 16:3363b9f14913 1235 }
tnhnrl 16:3363b9f14913 1236 else {
tnhnrl 16:3363b9f14913 1237 pc().printf("This key does nothing here.\r");
tnhnrl 16:3363b9f14913 1238 }
tnhnrl 16:3363b9f14913 1239 }
tnhnrl 16:3363b9f14913 1240 }
tnhnrl 20:8987a9ae2bc7 1241
tnhnrl 16:3363b9f14913 1242 void StateMachine::keyboard_menu_DEPTH_PID_settings() {
tnhnrl 16:3363b9f14913 1243 char PID_key;
tnhnrl 16:3363b9f14913 1244 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 16:3363b9f14913 1245
tnhnrl 16:3363b9f14913 1246 // show the menu
tnhnrl 16:3363b9f14913 1247 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 1248 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 1249 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\n\n\r");
tnhnrl 16:3363b9f14913 1250 pc().printf("depth P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 1251
tnhnrl 16:3363b9f14913 1252 // handle the key presses
tnhnrl 16:3363b9f14913 1253 while(1) {
tnhnrl 16:3363b9f14913 1254 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 1255 if (pc().readable()) {
tnhnrl 16:3363b9f14913 1256 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 1257 }
tnhnrl 16:3363b9f14913 1258 else {
tnhnrl 16:3363b9f14913 1259 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 1260 }
tnhnrl 16:3363b9f14913 1261
tnhnrl 16:3363b9f14913 1262 // handle the user's key input
tnhnrl 16:3363b9f14913 1263 if (PID_key == '-') {
tnhnrl 21:38c8544db6f4 1264 _depth_KP -= gain_step_size;
tnhnrl 21:38c8544db6f4 1265 pc().printf("P gain: %0.5f \r\n", _depth_KP);
tnhnrl 16:3363b9f14913 1266 }
tnhnrl 16:3363b9f14913 1267 else if (PID_key == '=') {
tnhnrl 21:38c8544db6f4 1268 _depth_KP += gain_step_size;
tnhnrl 21:38c8544db6f4 1269 pc().printf("P gain: %0.5f \r\n", _depth_KP);
tnhnrl 16:3363b9f14913 1270 }
tnhnrl 16:3363b9f14913 1271 else if (PID_key == '[') {
tnhnrl 21:38c8544db6f4 1272 _depth_KI -= gain_step_size;
tnhnrl 21:38c8544db6f4 1273 pc().printf("I gain: %0.5f \r\n", _depth_KI);
tnhnrl 16:3363b9f14913 1274 }
tnhnrl 16:3363b9f14913 1275 else if (PID_key == ']') {
tnhnrl 21:38c8544db6f4 1276 _depth_KI += gain_step_size;
tnhnrl 21:38c8544db6f4 1277 pc().printf("I gain: %0.5f \r\n", _depth_KI);
tnhnrl 16:3363b9f14913 1278 }
tnhnrl 16:3363b9f14913 1279 else if (PID_key == ';') {
tnhnrl 21:38c8544db6f4 1280 _depth_KD -= gain_step_size;
tnhnrl 21:38c8544db6f4 1281 pc().printf("D gain: %0.5f \r\n", _depth_KD);
tnhnrl 16:3363b9f14913 1282 }
tnhnrl 16:3363b9f14913 1283 else if (PID_key == '\'') {
tnhnrl 21:38c8544db6f4 1284 _depth_KD += gain_step_size;
tnhnrl 21:38c8544db6f4 1285 pc().printf("D gain: %0.5f \r\n", _depth_KD);
tnhnrl 16:3363b9f14913 1286 }
tnhnrl 16:3363b9f14913 1287 else if (PID_key == 'S') { // user wants to save these settings
tnhnrl 16:3363b9f14913 1288 // set global values
tnhnrl 21:38c8544db6f4 1289 depthLoop().setControllerP(_depth_KP);
tnhnrl 21:38c8544db6f4 1290 depthLoop().setControllerI(_depth_KI);
tnhnrl 21:38c8544db6f4 1291 depthLoop().setControllerD(_depth_KD);
tnhnrl 16:3363b9f14913 1292
tnhnrl 21:38c8544db6f4 1293 // save depth PID values for outer loop
tnhnrl 21:38c8544db6f4 1294 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
tnhnrl 16:3363b9f14913 1295 break; //exit the while loop
tnhnrl 16:3363b9f14913 1296 }
tnhnrl 16:3363b9f14913 1297 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 1298 break; //exit the while loop
tnhnrl 16:3363b9f14913 1299 }
tnhnrl 16:3363b9f14913 1300 else {
tnhnrl 16:3363b9f14913 1301 pc().printf("\n\rThis key does nothing here. ");
tnhnrl 16:3363b9f14913 1302 }
tnhnrl 16:3363b9f14913 1303 }
tnhnrl 16:3363b9f14913 1304 }
tnhnrl 16:3363b9f14913 1305
tnhnrl 16:3363b9f14913 1306 void StateMachine::keyboard_menu_PITCH_PID_settings() {
tnhnrl 16:3363b9f14913 1307 char PID_key;
tnhnrl 16:3363b9f14913 1308 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 16:3363b9f14913 1309
tnhnrl 16:3363b9f14913 1310 // print the menu
tnhnrl 16:3363b9f14913 1311 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 1312 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 1313 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
tnhnrl 16:3363b9f14913 1314 pc().printf("pitch P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
tnhnrl 20:8987a9ae2bc7 1315
tnhnrl 16:3363b9f14913 1316 // handle the key presses
tnhnrl 16:3363b9f14913 1317 while(1) {
tnhnrl 16:3363b9f14913 1318 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 1319 if (pc().readable()) {
tnhnrl 16:3363b9f14913 1320 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 1321 }
tnhnrl 16:3363b9f14913 1322 else {
tnhnrl 16:3363b9f14913 1323 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 1324 }
tnhnrl 16:3363b9f14913 1325
tnhnrl 16:3363b9f14913 1326 // handle the user's key input
tnhnrl 16:3363b9f14913 1327 if (PID_key == '-') {
tnhnrl 21:38c8544db6f4 1328 _pitch_KP -= gain_step_size;
tnhnrl 21:38c8544db6f4 1329 pc().printf("\rP gain: %0.5f ", _pitch_KP);
tnhnrl 16:3363b9f14913 1330 }
tnhnrl 16:3363b9f14913 1331 else if (PID_key == '=') {
tnhnrl 21:38c8544db6f4 1332 _pitch_KP += gain_step_size;
tnhnrl 21:38c8544db6f4 1333 pc().printf("\rP gain: %0.5f ", _pitch_KP);
tnhnrl 16:3363b9f14913 1334 }
tnhnrl 16:3363b9f14913 1335 else if (PID_key == '[') {
tnhnrl 21:38c8544db6f4 1336 _pitch_KI -= gain_step_size;
tnhnrl 21:38c8544db6f4 1337 pc().printf("\rI gain: %0.5f ", _pitch_KI);
tnhnrl 16:3363b9f14913 1338 }
tnhnrl 16:3363b9f14913 1339 else if (PID_key == ']') {
tnhnrl 21:38c8544db6f4 1340 _pitch_KI += gain_step_size;
tnhnrl 21:38c8544db6f4 1341 pc().printf("\rI gain: %0.5f ", _pitch_KI);
tnhnrl 16:3363b9f14913 1342 }
tnhnrl 16:3363b9f14913 1343 else if (PID_key == ';') {
tnhnrl 21:38c8544db6f4 1344 _pitch_KD -= gain_step_size;
tnhnrl 21:38c8544db6f4 1345 pc().printf("\rD gain: %0.5f ", _pitch_KD);
tnhnrl 16:3363b9f14913 1346 }
tnhnrl 16:3363b9f14913 1347 else if (PID_key == '\'') {
tnhnrl 21:38c8544db6f4 1348 _pitch_KD += gain_step_size;
tnhnrl 21:38c8544db6f4 1349 pc().printf("\rD gain: %0.5f ", _pitch_KD);
tnhnrl 16:3363b9f14913 1350 }
tnhnrl 16:3363b9f14913 1351 else if (PID_key == 'S') { // user wants to save the modified values
tnhnrl 16:3363b9f14913 1352 // set global values
tnhnrl 21:38c8544db6f4 1353 pitchLoop().setControllerP(_pitch_KP);
tnhnrl 21:38c8544db6f4 1354 pitchLoop().setControllerI(_pitch_KI);
tnhnrl 21:38c8544db6f4 1355 pitchLoop().setControllerD(_pitch_KD);
tnhnrl 16:3363b9f14913 1356
tnhnrl 32:f2f8ae34aadc 1357 // save pitch PID values for outer loop (must save neutral position also)
tnhnrl 21:38c8544db6f4 1358 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
tnhnrl 16:3363b9f14913 1359 break; //exit the while loop
tnhnrl 16:3363b9f14913 1360 }
tnhnrl 16:3363b9f14913 1361 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 1362 break; //exit the while loop
tnhnrl 16:3363b9f14913 1363 }
tnhnrl 16:3363b9f14913 1364 else {
tnhnrl 16:3363b9f14913 1365 pc().printf("This key does nothing here.\r");
tnhnrl 16:3363b9f14913 1366 }
tnhnrl 16:3363b9f14913 1367 }
tnhnrl 16:3363b9f14913 1368 }
tnhnrl 20:8987a9ae2bc7 1369
tnhnrl 16:3363b9f14913 1370 float StateMachine::getDepthCommand() {
tnhnrl 32:f2f8ae34aadc 1371 return _depth_command;
tnhnrl 16:3363b9f14913 1372 }
tnhnrl 20:8987a9ae2bc7 1373
tnhnrl 16:3363b9f14913 1374 float StateMachine::getPitchCommand() {
tnhnrl 32:f2f8ae34aadc 1375 return _pitch_command;
tnhnrl 32:f2f8ae34aadc 1376 }
tnhnrl 32:f2f8ae34aadc 1377
tnhnrl 32:f2f8ae34aadc 1378 float StateMachine::getDepthReading() {
tnhnrl 32:f2f8ae34aadc 1379 return _depth_reading;
tnhnrl 32:f2f8ae34aadc 1380 }
tnhnrl 32:f2f8ae34aadc 1381
tnhnrl 32:f2f8ae34aadc 1382 float StateMachine::getPitchReading() {
tnhnrl 32:f2f8ae34aadc 1383 return _pitch_reading;
tnhnrl 32:f2f8ae34aadc 1384 }
tnhnrl 32:f2f8ae34aadc 1385
tnhnrl 32:f2f8ae34aadc 1386 float StateMachine::getTimerReading() {
tnhnrl 32:f2f8ae34aadc 1387 return _timer_reading;
tnhnrl 17:7c16b5671d0e 1388 }
tnhnrl 28:16c83a2fdefa 1389
tnhnrl 17:7c16b5671d0e 1390 void StateMachine::setState(int input_state) {
tnhnrl 21:38c8544db6f4 1391 _state = input_state;
tnhnrl 17:7c16b5671d0e 1392 }
tnhnrl 20:8987a9ae2bc7 1393
tnhnrl 17:7c16b5671d0e 1394 int StateMachine::getState() {
tnhnrl 17:7c16b5671d0e 1395 return _state; //return the current state of the system
tnhnrl 17:7c16b5671d0e 1396 }
tnhnrl 20:8987a9ae2bc7 1397
tnhnrl 17:7c16b5671d0e 1398 void StateMachine::setTimeout(float input_timeout) {
tnhnrl 17:7c16b5671d0e 1399 _timeout = input_timeout;
tnhnrl 17:7c16b5671d0e 1400 }
tnhnrl 20:8987a9ae2bc7 1401
tnhnrl 17:7c16b5671d0e 1402 void StateMachine::setDepthCommand(float input_depth_command) {
tnhnrl 32:f2f8ae34aadc 1403 _depth_command = input_depth_command;
tnhnrl 17:7c16b5671d0e 1404 }
tnhnrl 20:8987a9ae2bc7 1405
tnhnrl 17:7c16b5671d0e 1406 void StateMachine::setPitchCommand(float input_pitch_command) {
tnhnrl 32:f2f8ae34aadc 1407 _pitch_command = input_pitch_command;
tnhnrl 17:7c16b5671d0e 1408 }
tnhnrl 20:8987a9ae2bc7 1409
tnhnrl 17:7c16b5671d0e 1410 void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) {
tnhnrl 21:38c8544db6f4 1411 _neutral_batt_pos_mm = batt_pos_mm;
tnhnrl 21:38c8544db6f4 1412 _neutral_bce_pos_mm = bce_pos_mm;
tnhnrl 17:7c16b5671d0e 1413
tnhnrl 21:38c8544db6f4 1414 pc().printf("Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\n\r",_neutral_batt_pos_mm,_neutral_bce_pos_mm);
tnhnrl 17:7c16b5671d0e 1415 }
tnhnrl 20:8987a9ae2bc7 1416
tnhnrl 17:7c16b5671d0e 1417 int StateMachine::timeoutRunning() {
tnhnrl 28:16c83a2fdefa 1418 return _isTimeoutRunning;
tnhnrl 17:7c16b5671d0e 1419 }
tnhnrl 20:8987a9ae2bc7 1420
tnhnrl 17:7c16b5671d0e 1421 //process one state at a time
tnhnrl 17:7c16b5671d0e 1422 void StateMachine::getDiveSequence() {
tnhnrl 17:7c16b5671d0e 1423 //iterate through this sequence using the FSM
tnhnrl 24:c7d9b5bf3829 1424 currentStateStruct.state = sequenceController().sequenceStructLoaded[_multi_dive_counter].state;
tnhnrl 24:c7d9b5bf3829 1425 currentStateStruct.timeout = sequenceController().sequenceStructLoaded[_multi_dive_counter].timeout;
tnhnrl 24:c7d9b5bf3829 1426 currentStateStruct.depth = sequenceController().sequenceStructLoaded[_multi_dive_counter].depth;
tnhnrl 24:c7d9b5bf3829 1427 currentStateStruct.pitch = sequenceController().sequenceStructLoaded[_multi_dive_counter].pitch;
tnhnrl 17:7c16b5671d0e 1428
tnhnrl 17:7c16b5671d0e 1429 _timeout = currentStateStruct.timeout; //set timeout before exiting this function
tnhnrl 32:f2f8ae34aadc 1430 }
tnhnrl 32:f2f8ae34aadc 1431
tnhnrl 32:f2f8ae34aadc 1432 void StateMachine::printCurrentSdLog() {
tnhnrl 32:f2f8ae34aadc 1433 pc().printf("SD card log work in progress\n\r");
tnhnrl 32:f2f8ae34aadc 1434 //might be worth saving the last few logs to the MBED...
tnhnrl 32:f2f8ae34aadc 1435 }
tnhnrl 32:f2f8ae34aadc 1436
tnhnrl 32:f2f8ae34aadc 1437 //check if the file is still opened
tnhnrl 32:f2f8ae34aadc 1438 void StateMachine::createNewFile() {
tnhnrl 32:f2f8ae34aadc 1439 if (_file_closed) {
tnhnrl 34:9b66c5188051 1440 //mbedLogger().createFile(); //create a new MBED file
tnhnrl 32:f2f8ae34aadc 1441
tnhnrl 32:f2f8ae34aadc 1442 _file_closed = false; //file is still open until you get to SIT_IDLE
tnhnrl 32:f2f8ae34aadc 1443 }
tnhnrl 32:f2f8ae34aadc 1444 }
tnhnrl 32:f2f8ae34aadc 1445
tnhnrl 32:f2f8ae34aadc 1446 void StateMachine::transmitData() {
tnhnrl 32:f2f8ae34aadc 1447 static float transmit_timer = 0;
tnhnrl 32:f2f8ae34aadc 1448 static bool is_transmit_timer_running = false;
tnhnrl 32:f2f8ae34aadc 1449
tnhnrl 32:f2f8ae34aadc 1450 if (!is_transmit_timer_running) {
tnhnrl 32:f2f8ae34aadc 1451 //pc().printf("\n\n\rTRANSMIT timer running...\n\n\r"); //debug
tnhnrl 32:f2f8ae34aadc 1452
tnhnrl 32:f2f8ae34aadc 1453 transmit_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds
tnhnrl 32:f2f8ae34aadc 1454 is_transmit_timer_running = true; //disable this block after one iteration
tnhnrl 32:f2f8ae34aadc 1455
tnhnrl 32:f2f8ae34aadc 1456 pc().printf("TESTING to see if this transmits once a second. (timer: %0.1f)\n\r", timer.read());
tnhnrl 32:f2f8ae34aadc 1457 }
tnhnrl 32:f2f8ae34aadc 1458 if (timer.read() >= transmit_timer) {
tnhnrl 32:f2f8ae34aadc 1459 is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 32:f2f8ae34aadc 1460 }
tnhnrl 34:9b66c5188051 1461 }
tnhnrl 34:9b66c5188051 1462
tnhnrl 34:9b66c5188051 1463 void StateMachine::recordData(int input_state) {
tnhnrl 34:9b66c5188051 1464 string string_state;
tnhnrl 34:9b66c5188051 1465 if (input_state == SIT_IDLE)
tnhnrl 34:9b66c5188051 1466 string_state = "SIT_IDLE";
tnhnrl 34:9b66c5188051 1467 else if (input_state == FIND_NEUTRAL)
tnhnrl 34:9b66c5188051 1468 string_state = "FIND_NEUTRAL";
tnhnrl 34:9b66c5188051 1469 else if (input_state == DIVE)
tnhnrl 34:9b66c5188051 1470 string_state = "DIVE";
tnhnrl 34:9b66c5188051 1471 else if (input_state == RISE)
tnhnrl 34:9b66c5188051 1472 string_state = "RISE";
tnhnrl 34:9b66c5188051 1473 else if (input_state == FLOAT_LEVEL)
tnhnrl 34:9b66c5188051 1474 string_state = "FLOAT_LEVEL";
tnhnrl 34:9b66c5188051 1475 else if (input_state == FLOAT_BROADCAST)
tnhnrl 34:9b66c5188051 1476 string_state = "FLOAT_BROADCAST";
tnhnrl 34:9b66c5188051 1477 else if (input_state == EMERGENCY_CLIMB)
tnhnrl 34:9b66c5188051 1478 string_state = "EMERGENCY_CLIMB";
tnhnrl 34:9b66c5188051 1479 else if (input_state == MULTI_DIVE)
tnhnrl 34:9b66c5188051 1480 string_state = "MULTI_DIVE";
tnhnrl 34:9b66c5188051 1481 else if (input_state == MULTI_RISE)
tnhnrl 34:9b66c5188051 1482 string_state = "MULTI_RISE";
tnhnrl 34:9b66c5188051 1483 else if (input_state == KEYBOARD)
tnhnrl 34:9b66c5188051 1484 string_state = "KEYBOARD";
tnhnrl 34:9b66c5188051 1485
tnhnrl 34:9b66c5188051 1486 if (!_is_log_timer_running) {
tnhnrl 34:9b66c5188051 1487 //pc().printf("\n\n\rlog timer running...\n\n\r"); //debug
tnhnrl 34:9b66c5188051 1488
tnhnrl 34:9b66c5188051 1489 _log_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds
tnhnrl 34:9b66c5188051 1490 _is_log_timer_running = true; //disable this block after one iteration
tnhnrl 34:9b66c5188051 1491
tnhnrl 37:357e98a929cc 1492 _data_log[0] = systemTime().read(); //system time reading
tnhnrl 34:9b66c5188051 1493 _data_log[1] = depthLoop().getCommand(); //depth command
tnhnrl 34:9b66c5188051 1494 _data_log[2] = depthLoop().getPosition(); //depth reading
tnhnrl 34:9b66c5188051 1495 _data_log[3] = pitchLoop().getCommand(); //pitch command
tnhnrl 34:9b66c5188051 1496 _data_log[4] = pitchLoop().getPosition(); //pitch reading
tnhnrl 34:9b66c5188051 1497 _data_log[5] = bce().getSetPosition_mm();
tnhnrl 34:9b66c5188051 1498 _data_log[6] = bce().getPosition_mm();
tnhnrl 34:9b66c5188051 1499 _data_log[7] = batt().getSetPosition_mm();
tnhnrl 34:9b66c5188051 1500 _data_log[8] = batt().getPosition_mm();
tnhnrl 34:9b66c5188051 1501
tnhnrl 34:9b66c5188051 1502 //record data to the MBED every 5 seconds
tnhnrl 35:2f66ea4863d5 1503 //mbedLogger().saveArrayToFile(string_state,input_state,_data_log);
tnhnrl 34:9b66c5188051 1504 }
tnhnrl 34:9b66c5188051 1505 if (timer.read() >= _log_timer) {
tnhnrl 34:9b66c5188051 1506 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 1507 }
tnhnrl 34:9b66c5188051 1508 }
tnhnrl 34:9b66c5188051 1509
tnhnrl 34:9b66c5188051 1510 void StateMachine::recordState(int input_state) {
tnhnrl 34:9b66c5188051 1511 string string_state;
tnhnrl 34:9b66c5188051 1512
tnhnrl 34:9b66c5188051 1513 if (input_state == SIT_IDLE)
tnhnrl 34:9b66c5188051 1514 string_state = "SIT_IDLE";
tnhnrl 34:9b66c5188051 1515 else if (input_state == FIND_NEUTRAL)
tnhnrl 34:9b66c5188051 1516 string_state = "FIND_NEUTRAL";
tnhnrl 34:9b66c5188051 1517 else if (input_state == DIVE)
tnhnrl 34:9b66c5188051 1518 string_state = "DIVE";
tnhnrl 34:9b66c5188051 1519 else if (input_state == RISE)
tnhnrl 34:9b66c5188051 1520 string_state = "RISE";
tnhnrl 34:9b66c5188051 1521 else if (input_state == FLOAT_LEVEL)
tnhnrl 34:9b66c5188051 1522 string_state = "FLOAT_LEVEL";
tnhnrl 34:9b66c5188051 1523 else if (input_state == FLOAT_BROADCAST)
tnhnrl 34:9b66c5188051 1524 string_state = "FLOAT_BROADCAST";
tnhnrl 34:9b66c5188051 1525 else if (input_state == EMERGENCY_CLIMB)
tnhnrl 34:9b66c5188051 1526 string_state = "EMERGENCY_CLIMB";
tnhnrl 34:9b66c5188051 1527 else if (input_state == MULTI_DIVE)
tnhnrl 34:9b66c5188051 1528 string_state = "MULTI_DIVE";
tnhnrl 34:9b66c5188051 1529 else if (input_state == MULTI_RISE)
tnhnrl 34:9b66c5188051 1530 string_state = "MULTI_RISE";
tnhnrl 34:9b66c5188051 1531 else if (input_state == KEYBOARD)
tnhnrl 34:9b66c5188051 1532 string_state = "KEYBOARD";
tnhnrl 34:9b66c5188051 1533 //datalogger().printf("%s\n", string_state.c_str());
tnhnrl 36:966a86937e17 1534 }
tnhnrl 36:966a86937e17 1535
tnhnrl 36:966a86937e17 1536 float * StateMachine::dataArray() {
tnhnrl 36:966a86937e17 1537 //return the array to a calling function
tnhnrl 36:966a86937e17 1538 return _data_log;
tnhnrl 16:3363b9f14913 1539 }