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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
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 52:f207567d3ea4 5 _timeout = 20; // 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 58:94b7fd55185e 14 _heading_command = 0.0;
tnhnrl 17:7c16b5671d0e 15
tnhnrl 57:ec69651c8c21 16 //new commands
tnhnrl 58:94b7fd55185e 17 _BCE_dive_offset = 0.0; //starting at the limits
tnhnrl 58:94b7fd55185e 18 _BMM_dive_offset = 0.0;
tnhnrl 57:ec69651c8c21 19 //new commands
tnhnrl 57:ec69651c8c21 20
tnhnrl 28:16c83a2fdefa 21 _neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM
tnhnrl 20:8987a9ae2bc7 22
tnhnrl 28:16c83a2fdefa 23 _state = SIT_IDLE; // select starting state here
tnhnrl 28:16c83a2fdefa 24 _isTimeoutRunning = false; // default timer to not running
tnhnrl 28:16c83a2fdefa 25 _isSubStateTimerRunning = false; // default timer to not running
tnhnrl 17:7c16b5671d0e 26
tnhnrl 24:c7d9b5bf3829 27 _multi_dive_counter = 0;
tnhnrl 21:38c8544db6f4 28
tnhnrl 21:38c8544db6f4 29 _neutral_sub_state_active = false;
tnhnrl 17:7c16b5671d0e 30
tnhnrl 21:38c8544db6f4 31 _depth_KP = depthLoop().getControllerP(); // load current depth value
tnhnrl 21:38c8544db6f4 32 _depth_KI = depthLoop().getControllerI(); // load current depth value
tnhnrl 21:38c8544db6f4 33 _depth_KD = depthLoop().getControllerD(); // load current depth value
tnhnrl 21:38c8544db6f4 34
tnhnrl 21:38c8544db6f4 35 _pitch_KP = pitchLoop().getControllerP(); // load current pitch value
tnhnrl 21:38c8544db6f4 36 _pitch_KI = pitchLoop().getControllerI(); // load current pitch value
tnhnrl 21:38c8544db6f4 37 _pitch_KD = pitchLoop().getControllerD(); // load current pitch value
tnhnrl 21:38c8544db6f4 38
tnhnrl 21:38c8544db6f4 39 _neutral_bce_pos_mm = depthLoop().getOutputOffset(); //load current neutral buoyancy position offset
tnhnrl 21:38c8544db6f4 40 _neutral_batt_pos_mm = pitchLoop().getOutputOffset(); //load current neutral buoyancy position offset
tnhnrl 23:434f04ef1fad 41
tnhnrl 28:16c83a2fdefa 42 _state_array_counter = 1; //used to iterate through and record states
tnhnrl 28:16c83a2fdefa 43 _substate_array_counter = 0; //used to iterate through and record substates
tnhnrl 28:16c83a2fdefa 44
tnhnrl 28:16c83a2fdefa 45 _state_array[0] = SIT_IDLE; //system starts in the SIT_IDLE state, record this
tnhnrl 24:c7d9b5bf3829 46
tnhnrl 30:2964617e7676 47 _substate = NEUTRAL_SINKING; //start sub-FSM in NEUTRAL_SINKING
tnhnrl 28:16c83a2fdefa 48 _previous_substate = -1; //to start sub-FSM
tnhnrl 28:16c83a2fdefa 49 _previous_state = -1; //for tracking FSM states
tnhnrl 28:16c83a2fdefa 50
tnhnrl 28:16c83a2fdefa 51 _max_recorded_depth_neutral = -99; //float to record max depth
tnhnrl 28:16c83a2fdefa 52 _max_recorded_depth_dive = -99; //float to record max depth
tnhnrl 32:f2f8ae34aadc 53
tnhnrl 32:f2f8ae34aadc 54 _neutral_sink_command_mm = -2.5; //defaults for neutral finding sub-FSM
tnhnrl 32:f2f8ae34aadc 55 _neutral_rise_command_mm = 2.0;
tnhnrl 32:f2f8ae34aadc 56 _neutral_pitch_command_mm = 0.5;
tnhnrl 32:f2f8ae34aadc 57
tnhnrl 32:f2f8ae34aadc 58 _max_recorded_auto_neutral_depth = -99;
tnhnrl 32:f2f8ae34aadc 59
tnhnrl 32:f2f8ae34aadc 60 _file_closed = true;
tnhnrl 57:ec69651c8c21 61
tnhnrl 57:ec69651c8c21 62 _debug_menu_on = false; //toggle between debug and simple menu screens
tnhnrl 16:3363b9f14913 63 }
tnhnrl 20:8987a9ae2bc7 64
tnhnrl 17:7c16b5671d0e 65 //Finite State Machine (FSM)
tnhnrl 45:16b8162188ca 66 int StateMachine::runStateMachine() {
tnhnrl 45:16b8162188ca 67 static int transmit_packet_number = 1; //for data transmission
tnhnrl 45:16b8162188ca 68
tnhnrl 16:3363b9f14913 69 // finite state machine ... each state has at least one exit criteria
tnhnrl 17:7c16b5671d0e 70 switch (_state) {
tnhnrl 16:3363b9f14913 71 case SIT_IDLE :
tnhnrl 28:16c83a2fdefa 72 case KEYBOARD:
tnhnrl 16:3363b9f14913 73 // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
tnhnrl 28:16c83a2fdefa 74 if (!_isTimeoutRunning) {
tnhnrl 57:ec69651c8c21 75
tnhnrl 57:ec69651c8c21 76 if (_debug_menu_on)
tnhnrl 57:ec69651c8c21 77 showDebugMenu();
tnhnrl 57:ec69651c8c21 78 else
tnhnrl 57:ec69651c8c21 79 showSimpleMenu();
tnhnrl 16:3363b9f14913 80 pc().printf("\r\n\nstate: SIT_IDLE\r\n");
tnhnrl 28:16c83a2fdefa 81 _isTimeoutRunning = true;
tnhnrl 20:8987a9ae2bc7 82
tnhnrl 16:3363b9f14913 83 // what is active?
tnhnrl 16:3363b9f14913 84 bce().pause();
tnhnrl 16:3363b9f14913 85 batt().pause();
tnhnrl 32:f2f8ae34aadc 86
tnhnrl 17:7c16b5671d0e 87 //reset sub FSM
tnhnrl 28:16c83a2fdefa 88 _isSubStateTimerRunning = false;
tnhnrl 32:f2f8ae34aadc 89
tnhnrl 32:f2f8ae34aadc 90 //close the MBED file
tnhnrl 32:f2f8ae34aadc 91 _file_closed = true;
tnhnrl 16:3363b9f14913 92 }
tnhnrl 20:8987a9ae2bc7 93
tnhnrl 16:3363b9f14913 94 // how exit?
tnhnrl 20:8987a9ae2bc7 95 keyboard(); // keyboard function will change the state if needed
tnhnrl 16:3363b9f14913 96 break;
tnhnrl 49:47ffa4feb6db 97
tnhnrl 49:47ffa4feb6db 98 case CHECK_TUNING : // state used to check the tuning of the pressure vessel
tnhnrl 49:47ffa4feb6db 99 // start local state timer and init any other one-shot actions
tnhnrl 49:47ffa4feb6db 100 if (!_isTimeoutRunning) {
tnhnrl 49:47ffa4feb6db 101 pc().printf("\r\n\nstate: CHECK_TUNING\r\n");
tnhnrl 49:47ffa4feb6db 102 timer.reset(); // timer goes back to zero
tnhnrl 49:47ffa4feb6db 103 timer.start(); // background timer starts running
tnhnrl 49:47ffa4feb6db 104 _isTimeoutRunning = true;
tnhnrl 49:47ffa4feb6db 105
tnhnrl 49:47ffa4feb6db 106 // what needs to be started?
tnhnrl 49:47ffa4feb6db 107 bce().unpause(); //this is now active
tnhnrl 49:47ffa4feb6db 108 batt().unpause(); //this is now active
tnhnrl 49:47ffa4feb6db 109
tnhnrl 49:47ffa4feb6db 110 // what are the commands? (DRIVE THE MOTORS "DIRECTLY")
tnhnrl 49:47ffa4feb6db 111 bce().setPosition_mm(_neutral_bce_pos_mm); //this variable is loaded from the file at initialization
tnhnrl 49:47ffa4feb6db 112 batt().setPosition_mm(_neutral_batt_pos_mm); //this variable is loaded from the file at initialization
tnhnrl 49:47ffa4feb6db 113
tnhnrl 49:47ffa4feb6db 114 // getSetPosition_mm is the commanded position in the LinearActuator class
tnhnrl 49:47ffa4feb6db 115
tnhnrl 49:47ffa4feb6db 116 pc().printf("CHECK_TUNING: BCE cmd: %3.1f (BCE current position: %3.1f)\r\n", bce().getSetPosition_mm(), bce().getPosition_mm());
tnhnrl 49:47ffa4feb6db 117 pc().printf("CHECK_TUNING: BATT cmd: %3.1f (BATT current position: %3.1f)\r\n", batt().getSetPosition_mm(), bce().getPosition_mm());
tnhnrl 49:47ffa4feb6db 118
tnhnrl 49:47ffa4feb6db 119 //show that this is the start of new dive sequence
tnhnrl 49:47ffa4feb6db 120 recordState(_state);
tnhnrl 49:47ffa4feb6db 121
tnhnrl 49:47ffa4feb6db 122 //triggers logger array
tnhnrl 49:47ffa4feb6db 123 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 49:47ffa4feb6db 124 recordData(_state);
tnhnrl 49:47ffa4feb6db 125 }
tnhnrl 49:47ffa4feb6db 126
tnhnrl 49:47ffa4feb6db 127 // how exit?
tnhnrl 49:47ffa4feb6db 128 if (timer > _timeout) {
tnhnrl 49:47ffa4feb6db 129 pc().printf("CHECK_TUNING: timed out!\r\n");
tnhnrl 49:47ffa4feb6db 130 _state = FLOAT_BROADCAST;
tnhnrl 49:47ffa4feb6db 131 timer.reset();
tnhnrl 49:47ffa4feb6db 132 _isTimeoutRunning = false;
tnhnrl 49:47ffa4feb6db 133 }
tnhnrl 49:47ffa4feb6db 134
tnhnrl 49:47ffa4feb6db 135 //print status to screen continuously
tnhnrl 49:47ffa4feb6db 136 pc().printf("CHECK_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (BCE_cmd: %0.1f, BATT_cmd: %0.1f)(depth: %0.1f ft, pitch: %0.1f deg) [%0.1f sec]\r",bce().getPosition_mm(),batt().getPosition_mm(),bce().getSetPosition_mm(),batt().getSetPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(),timer.read());
tnhnrl 49:47ffa4feb6db 137
tnhnrl 49:47ffa4feb6db 138 //record data every 5 seconds
tnhnrl 49:47ffa4feb6db 139 recordData(_state);
tnhnrl 49:47ffa4feb6db 140
tnhnrl 49:47ffa4feb6db 141 break;
tnhnrl 20:8987a9ae2bc7 142
tnhnrl 16:3363b9f14913 143 case EMERGENCY_CLIMB :
tnhnrl 16:3363b9f14913 144 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 145 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 146 pc().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n");
tnhnrl 16:3363b9f14913 147 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 148 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 149 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 150
tnhnrl 16:3363b9f14913 151 // what needs to be started?
tnhnrl 16:3363b9f14913 152 bce().unpause();
tnhnrl 16:3363b9f14913 153 batt().unpause();
tnhnrl 20:8987a9ae2bc7 154
tnhnrl 20:8987a9ae2bc7 155 // what are the commands?
tnhnrl 16:3363b9f14913 156 bce().setPosition_mm(bce().getTravelLimit());
tnhnrl 16:3363b9f14913 157 batt().setPosition_mm(0.0);
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 EMERGENCY_CLIMB sequence
tnhnrl 32:f2f8ae34aadc 163 recordState(_state);
tnhnrl 32:f2f8ae34aadc 164
tnhnrl 49:47ffa4feb6db 165 //triggers logger array
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 16:3363b9f14913 170 // how exit?
tnhnrl 17:7c16b5671d0e 171 if (timer > _timeout) {
tnhnrl 16:3363b9f14913 172 pc().printf("EC: timed out\r\n");
tnhnrl 21:38c8544db6f4 173 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 174 timer.reset();
tnhnrl 28:16c83a2fdefa 175 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 176 }
tnhnrl 26:7e118fc02eea 177 else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
tnhnrl 21:38c8544db6f4 178 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 179 timer.reset();
tnhnrl 28:16c83a2fdefa 180 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 181 }
tnhnrl 32:f2f8ae34aadc 182
tnhnrl 34:9b66c5188051 183 //print status to screen continuously
tnhnrl 34:9b66c5188051 184 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 185
tnhnrl 32:f2f8ae34aadc 186 //record data every 5 seconds
tnhnrl 34:9b66c5188051 187 recordData(_state);
tnhnrl 32:f2f8ae34aadc 188
tnhnrl 16:3363b9f14913 189 break;
tnhnrl 20:8987a9ae2bc7 190
tnhnrl 16:3363b9f14913 191 case FIND_NEUTRAL :
tnhnrl 20:8987a9ae2bc7 192 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 193 if (!_isTimeoutRunning) {
tnhnrl 58:94b7fd55185e 194 pc().printf("\r\n\nstate: FIND_NEUTRAL\r\n");
tnhnrl 16:3363b9f14913 195 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 196 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 197 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 198
tnhnrl 16:3363b9f14913 199 // what needs to be started?
tnhnrl 16:3363b9f14913 200 bce().unpause();
tnhnrl 16:3363b9f14913 201 batt().unpause();
tnhnrl 28:16c83a2fdefa 202 bce().setPosition_mm(_bceFloatPosition);
tnhnrl 24:c7d9b5bf3829 203 batt().setPosition_mm(_neutral_batt_pos_mm); //set battery to close-to-neutral setting from config file
tnhnrl 17:7c16b5671d0e 204
tnhnrl 24:c7d9b5bf3829 205 //first iteration goes into Neutral Finding Sub-FSM
tnhnrl 24:c7d9b5bf3829 206 //set the first state of the FSM, and start the sub-FSM
tnhnrl 30:2964617e7676 207 _substate = NEUTRAL_SINKING; //first state in neutral sub-FSM is the pressure vessel sinking
tnhnrl 28:16c83a2fdefa 208 _previous_substate = -1;
tnhnrl 28:16c83a2fdefa 209
tnhnrl 28:16c83a2fdefa 210 //save this state to the array
tnhnrl 30:2964617e7676 211 _substate_array[_substate_array_counter] = NEUTRAL_SINKING; //save to state array
tnhnrl 28:16c83a2fdefa 212 _substate_array_counter++;
tnhnrl 28:16c83a2fdefa 213
tnhnrl 32:f2f8ae34aadc 214 runNeutralStateMachine();
tnhnrl 32:f2f8ae34aadc 215
tnhnrl 32:f2f8ae34aadc 216 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 217 //createNewFile();
tnhnrl 32:f2f8ae34aadc 218
tnhnrl 32:f2f8ae34aadc 219 //show that this is the start of a new FIND_NEUTRAL sequence
tnhnrl 32:f2f8ae34aadc 220 recordState(_state);
tnhnrl 32:f2f8ae34aadc 221
tnhnrl 49:47ffa4feb6db 222 //triggers logger array
tnhnrl 32:f2f8ae34aadc 223 _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 224 recordData(_state);
tnhnrl 16:3363b9f14913 225 }
tnhnrl 20:8987a9ae2bc7 226
tnhnrl 20:8987a9ae2bc7 227 // how exit? (exit with the timer, if timer still running continue processing sub FSM)
tnhnrl 17:7c16b5671d0e 228 if (timer > _timeout) {
tnhnrl 17:7c16b5671d0e 229 pc().printf("FN: timed out [time: %0.1f sec]\r\n", timer.read());
tnhnrl 21:38c8544db6f4 230 _state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces)
tnhnrl 16:3363b9f14913 231 timer.reset();
tnhnrl 28:16c83a2fdefa 232 _isTimeoutRunning = false;
tnhnrl 24:c7d9b5bf3829 233
tnhnrl 24:c7d9b5bf3829 234 //record this to the NEUTRAL sub-FSM tracker
tnhnrl 28:16c83a2fdefa 235 _substate_array[_substate_array_counter] = EMERGENCY_CLIMB; //save to state array
tnhnrl 28:16c83a2fdefa 236 _substate_array_counter++;
tnhnrl 16:3363b9f14913 237 }
tnhnrl 21:38c8544db6f4 238
tnhnrl 24:c7d9b5bf3829 239 //what is active? (neutral finding sub-function runs until completion)
tnhnrl 24:c7d9b5bf3829 240 //check if substate returned exit state, if so stop running the sub-FSM
tnhnrl 26:7e118fc02eea 241 else if (runNeutralStateMachine() == NEUTRAL_EXIT) {
tnhnrl 21:38c8544db6f4 242 //if successful, FIND_NEUTRAL then goes to RISE
tnhnrl 58:94b7fd55185e 243 pc().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\r\n\n");
tnhnrl 21:38c8544db6f4 244 _state = RISE;
tnhnrl 30:2964617e7676 245 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 246 }
tnhnrl 32:f2f8ae34aadc 247
tnhnrl 32:f2f8ae34aadc 248 //record data every 5 seconds
tnhnrl 34:9b66c5188051 249 recordData(_state);
tnhnrl 32:f2f8ae34aadc 250
tnhnrl 17:7c16b5671d0e 251 break;
tnhnrl 17:7c16b5671d0e 252
tnhnrl 16:3363b9f14913 253 case DIVE :
tnhnrl 16:3363b9f14913 254 // start local state timer and init any other one-shot actions
tnhnrl 32:f2f8ae34aadc 255
tnhnrl 28:16c83a2fdefa 256 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 257 pc().printf("\r\n\nstate: DIVE\r\n");
tnhnrl 16:3363b9f14913 258 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 259 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 260 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 261
tnhnrl 16:3363b9f14913 262 // what needs to be started?
tnhnrl 16:3363b9f14913 263 bce().unpause();
tnhnrl 16:3363b9f14913 264 batt().unpause();
tnhnrl 20:8987a9ae2bc7 265
tnhnrl 16:3363b9f14913 266 // what are the commands?
tnhnrl 32:f2f8ae34aadc 267 depthLoop().setCommand(_depth_command);
tnhnrl 32:f2f8ae34aadc 268 pitchLoop().setCommand(_pitch_command);
tnhnrl 32:f2f8ae34aadc 269
tnhnrl 34:9b66c5188051 270 pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
tnhnrl 34:9b66c5188051 271 pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 28:16c83a2fdefa 272
tnhnrl 28:16c83a2fdefa 273 //reset max dive depth
tnhnrl 28:16c83a2fdefa 274 _max_recorded_depth_dive = -99; //float to record max depth
tnhnrl 32:f2f8ae34aadc 275
tnhnrl 32:f2f8ae34aadc 276 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 277 //createNewFile();
tnhnrl 32:f2f8ae34aadc 278
tnhnrl 32:f2f8ae34aadc 279 //show that this is the start of new dive sequence
tnhnrl 32:f2f8ae34aadc 280 recordState(_state);
tnhnrl 32:f2f8ae34aadc 281
tnhnrl 49:47ffa4feb6db 282 //triggers logger array
tnhnrl 32:f2f8ae34aadc 283 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 284 recordData(_state);
tnhnrl 16:3363b9f14913 285 }
tnhnrl 20:8987a9ae2bc7 286
tnhnrl 16:3363b9f14913 287 // how exit?
tnhnrl 32:f2f8ae34aadc 288 if (timer.read() > _timeout) {
tnhnrl 58:94b7fd55185e 289 pc().printf("DIVE: timed out\r\n\n");
tnhnrl 21:38c8544db6f4 290 _state = RISE; //new behavior 11/17/2017
tnhnrl 16:3363b9f14913 291 timer.reset();
tnhnrl 28:16c83a2fdefa 292 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 293 }
tnhnrl 32:f2f8ae34aadc 294 else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches
tnhnrl 32:f2f8ae34aadc 295 pc().printf("DIVE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 21:38c8544db6f4 296 _state = RISE;
tnhnrl 16:3363b9f14913 297 timer.reset();
tnhnrl 28:16c83a2fdefa 298 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 299 }
tnhnrl 20:8987a9ae2bc7 300
tnhnrl 16:3363b9f14913 301 // what is active?
tnhnrl 58:94b7fd55185e 302 pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depth: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read(),bce().getSetPosition_mm(),batt().getSetPosition_mm());
tnhnrl 32:f2f8ae34aadc 303 bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
tnhnrl 16:3363b9f14913 304 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 28:16c83a2fdefa 305
tnhnrl 28:16c83a2fdefa 306 if (depthLoop().getPosition() > _max_recorded_depth_dive) { //debug
tnhnrl 28:16c83a2fdefa 307 _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded
tnhnrl 28:16c83a2fdefa 308 }
tnhnrl 32:f2f8ae34aadc 309
tnhnrl 32:f2f8ae34aadc 310 //record data every 5 seconds
tnhnrl 34:9b66c5188051 311 recordData(_state);
tnhnrl 32:f2f8ae34aadc 312
tnhnrl 16:3363b9f14913 313 break;
tnhnrl 16:3363b9f14913 314
tnhnrl 16:3363b9f14913 315 case RISE :
tnhnrl 16:3363b9f14913 316 // start local state timer and init any other one-shot actions
tnhnrl 32:f2f8ae34aadc 317
tnhnrl 28:16c83a2fdefa 318 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 319 pc().printf("\r\n\nstate: RISE\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 16:3363b9f14913 328 // what are the commands?
tnhnrl 28:16c83a2fdefa 329 depthLoop().setCommand(-1.0); //make sure to get towards the surface (saw issues at LASR pool)
tnhnrl 32:f2f8ae34aadc 330 pitchLoop().setCommand(-_pitch_command);
tnhnrl 34:9b66c5188051 331
tnhnrl 34:9b66c5188051 332 pc().printf("RISE: depth cmd: %0.1f\r\n",depthLoop().getCommand());
tnhnrl 34:9b66c5188051 333 pc().printf("RISE: pitch cmd: %0.1f\r\n",pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 334
tnhnrl 32:f2f8ae34aadc 335 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 336 //createNewFile();
tnhnrl 32:f2f8ae34aadc 337
tnhnrl 32:f2f8ae34aadc 338 //show that this is the start of new rise sequence
tnhnrl 32:f2f8ae34aadc 339 recordState(_state);
tnhnrl 32:f2f8ae34aadc 340
tnhnrl 49:47ffa4feb6db 341 //triggers logger array
tnhnrl 32:f2f8ae34aadc 342 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 343 recordData(_state);
tnhnrl 16:3363b9f14913 344 }
tnhnrl 20:8987a9ae2bc7 345
tnhnrl 16:3363b9f14913 346 // how exit?
tnhnrl 32:f2f8ae34aadc 347 if (timer.read() > _timeout) {
tnhnrl 16:3363b9f14913 348 pc().printf("RISE: timed out\r\n");
tnhnrl 21:38c8544db6f4 349 _state = EMERGENCY_CLIMB;
tnhnrl 16:3363b9f14913 350 timer.reset();
tnhnrl 28:16c83a2fdefa 351 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 352 }
tnhnrl 32:f2f8ae34aadc 353
tnhnrl 32:f2f8ae34aadc 354 //modified from (depthLoop().getPosition() < depthLoop().getCommand() + 0.5)
tnhnrl 32:f2f8ae34aadc 355 //did not work correctly in bench test (stuck in rise state)
tnhnrl 32:f2f8ae34aadc 356 else if (depthLoop().getPosition() < 0.5) {
tnhnrl 32:f2f8ae34aadc 357 pc().printf("RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 28:16c83a2fdefa 358 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 359 timer.reset();
tnhnrl 28:16c83a2fdefa 360 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 361 }
tnhnrl 20:8987a9ae2bc7 362
tnhnrl 20:8987a9ae2bc7 363 // what is active?
tnhnrl 58:94b7fd55185e 364 pc().printf("RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read(),bce().getSetPosition_mm(),batt().getSetPosition_mm());
tnhnrl 17:7c16b5671d0e 365 bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
tnhnrl 32:f2f8ae34aadc 366 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 367
tnhnrl 32:f2f8ae34aadc 368 //record data every 5 seconds
tnhnrl 34:9b66c5188051 369 recordData(_state);
tnhnrl 32:f2f8ae34aadc 370
tnhnrl 58:94b7fd55185e 371 break;
tnhnrl 58:94b7fd55185e 372
tnhnrl 58:94b7fd55185e 373 // NEW DIVE AND RISE SEQUENCES
tnhnrl 58:94b7fd55185e 374 case POSITION_DIVE :
tnhnrl 58:94b7fd55185e 375 // start local state timer and init any other one-shot actions
tnhnrl 58:94b7fd55185e 376 if (!_isTimeoutRunning) {
tnhnrl 58:94b7fd55185e 377 pc().printf("\r\n\nstate: POSITION DIVE\r\n");
tnhnrl 58:94b7fd55185e 378 timer.reset(); // timer goes back to zero
tnhnrl 58:94b7fd55185e 379 timer.start(); // background timer starts running
tnhnrl 58:94b7fd55185e 380 _isTimeoutRunning = true;
tnhnrl 58:94b7fd55185e 381
tnhnrl 58:94b7fd55185e 382 // what needs to be started?
tnhnrl 58:94b7fd55185e 383 bce().unpause();
tnhnrl 58:94b7fd55185e 384 batt().unpause();
tnhnrl 58:94b7fd55185e 385 rudder().unpause();
tnhnrl 58:94b7fd55185e 386
tnhnrl 58:94b7fd55185e 387 // what are the commands? (using inner loops except for heading outer loop)
tnhnrl 58:94b7fd55185e 388 // These actions happen ONCE in the POSITION_DIVE sequence
tnhnrl 58:94b7fd55185e 389 batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset);
tnhnrl 58:94b7fd55185e 390 bce().setPosition_mm(_neutral_bce_pos_mm- _BMM_dive_offset);
tnhnrl 58:94b7fd55185e 391 //rudder().setPosition_deg(_heading_command);
tnhnrl 58:94b7fd55185e 392
tnhnrl 58:94b7fd55185e 393 headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
tnhnrl 58:94b7fd55185e 394
tnhnrl 58:94b7fd55185e 395 pc().printf("PD: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
tnhnrl 58:94b7fd55185e 396 pc().printf("PD: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
tnhnrl 58:94b7fd55185e 397 pc().printf("PD: heading cmd: %3.1f\r\n",headingLoop().getCommand());
tnhnrl 58:94b7fd55185e 398
tnhnrl 58:94b7fd55185e 399 //reset max dive depth
tnhnrl 58:94b7fd55185e 400 _max_recorded_depth_dive = -99; //float to record max depth
tnhnrl 58:94b7fd55185e 401
tnhnrl 58:94b7fd55185e 402 //show that this is the start of new dive sequence
tnhnrl 58:94b7fd55185e 403 recordState(_state);
tnhnrl 58:94b7fd55185e 404
tnhnrl 58:94b7fd55185e 405 //triggers logger array
tnhnrl 58:94b7fd55185e 406 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 58:94b7fd55185e 407 recordData(_state);
tnhnrl 58:94b7fd55185e 408 }
tnhnrl 58:94b7fd55185e 409
tnhnrl 58:94b7fd55185e 410 // how exit?
tnhnrl 58:94b7fd55185e 411 // timer runs out goes to POSITION_RISE
tnhnrl 58:94b7fd55185e 412 if (timer.read() > _timeout) {
tnhnrl 58:94b7fd55185e 413 pc().printf("PD: timed out\r\n\n");
tnhnrl 58:94b7fd55185e 414 _state = POSITION_RISE; //new behavior 11/17/2017
tnhnrl 58:94b7fd55185e 415 timer.reset();
tnhnrl 58:94b7fd55185e 416 _isTimeoutRunning = false;
tnhnrl 58:94b7fd55185e 417 }
tnhnrl 58:94b7fd55185e 418
tnhnrl 58:94b7fd55185e 419 // when you reach the dive threshold, surface
tnhnrl 58:94b7fd55185e 420 else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches
tnhnrl 58:94b7fd55185e 421 pc().printf("PD: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 58:94b7fd55185e 422 _state = POSITION_RISE;
tnhnrl 58:94b7fd55185e 423 timer.reset();
tnhnrl 58:94b7fd55185e 424 _isTimeoutRunning = false;
tnhnrl 58:94b7fd55185e 425 }
tnhnrl 58:94b7fd55185e 426
tnhnrl 58:94b7fd55185e 427 // what is active?
tnhnrl 58:94b7fd55185e 428 pc().printf("PD: bce pos (cmd pos): %3.1f mm (%0.1f), batt pos: %3.1f mm (%0.1f), rudder: %f (depth: %3.1f ft, pitch: %3.1f, heading: %3.1f [cmd: %0.1f])[%0.2f sec] [imu heading: %0.1f] \r", bce().getPosition_mm(),bce().getSetPosition_mm(), batt().getPosition_mm(),batt().getSetPosition_mm(), rudder().getPosition_pwm(), depthLoop().getPosition(), pitchLoop().getPosition(), headingLoop().getPosition(), headingLoop().getOutput(), timer.read(), imu().getHeading());
tnhnrl 58:94b7fd55185e 429
tnhnrl 58:94b7fd55185e 430 if (depthLoop().getPosition() > _max_recorded_depth_dive) {
tnhnrl 58:94b7fd55185e 431 _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded when it is larger than previous values
tnhnrl 58:94b7fd55185e 432 }
tnhnrl 58:94b7fd55185e 433
tnhnrl 58:94b7fd55185e 434 // ACTIVE RUDDER CONTROL
tnhnrl 58:94b7fd55185e 435 rudder().setPosition_deg(headingLoop().getOutput());
tnhnrl 58:94b7fd55185e 436
tnhnrl 58:94b7fd55185e 437 //record data internally (for access by MbedLogger)
tnhnrl 58:94b7fd55185e 438 recordData(_state);
tnhnrl 58:94b7fd55185e 439
tnhnrl 58:94b7fd55185e 440 break;
tnhnrl 58:94b7fd55185e 441
tnhnrl 58:94b7fd55185e 442 case POSITION_RISE :
tnhnrl 58:94b7fd55185e 443 // start local state timer and init any other one-shot actions
tnhnrl 58:94b7fd55185e 444
tnhnrl 58:94b7fd55185e 445 if (!_isTimeoutRunning) {
tnhnrl 58:94b7fd55185e 446 pc().printf("\r\n\nstate: POSITION RISE\r\n");
tnhnrl 58:94b7fd55185e 447 timer.reset(); // timer goes back to zero
tnhnrl 58:94b7fd55185e 448 timer.start(); // background timer starts running
tnhnrl 58:94b7fd55185e 449 _isTimeoutRunning = true;
tnhnrl 58:94b7fd55185e 450
tnhnrl 58:94b7fd55185e 451 // what needs to be started?
tnhnrl 58:94b7fd55185e 452 bce().unpause();
tnhnrl 58:94b7fd55185e 453 batt().unpause();
tnhnrl 58:94b7fd55185e 454
tnhnrl 58:94b7fd55185e 455 // what are the commands? (using inner loops except for heading outer loop)
tnhnrl 58:94b7fd55185e 456 pc().printf("PR: <<DEBUG>> CMD BCE: %0.1f, CMD BMM: %0.1f)\r\n", _BCE_dive_offset, _BMM_dive_offset);
tnhnrl 58:94b7fd55185e 457
tnhnrl 58:94b7fd55185e 458 batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset); //reversing the BCE and BATT positions
tnhnrl 58:94b7fd55185e 459 bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); //reversing the BCE and BATT positions
tnhnrl 58:94b7fd55185e 460 //rudder().setPosition_deg(_heading_command); //heading stays the same
tnhnrl 58:94b7fd55185e 461
tnhnrl 58:94b7fd55185e 462 headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
tnhnrl 58:94b7fd55185e 463
tnhnrl 58:94b7fd55185e 464 pc().printf("PR: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
tnhnrl 58:94b7fd55185e 465 pc().printf("PR: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
tnhnrl 58:94b7fd55185e 466 pc().printf("PR: heading cmd: %3.1f\r\n",headingLoop().getCommand());
tnhnrl 58:94b7fd55185e 467
tnhnrl 58:94b7fd55185e 468 //show that this is the start of new rise sequence
tnhnrl 58:94b7fd55185e 469 recordState(_state);
tnhnrl 58:94b7fd55185e 470
tnhnrl 58:94b7fd55185e 471 //triggers logger array
tnhnrl 58:94b7fd55185e 472 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 58:94b7fd55185e 473 recordData(_state);
tnhnrl 58:94b7fd55185e 474 }
tnhnrl 58:94b7fd55185e 475
tnhnrl 58:94b7fd55185e 476 // how exit?
tnhnrl 58:94b7fd55185e 477 if (timer.read() > _timeout) {
tnhnrl 58:94b7fd55185e 478 pc().printf("PR: timed out\r\n");
tnhnrl 58:94b7fd55185e 479 _state = EMERGENCY_CLIMB;
tnhnrl 58:94b7fd55185e 480 timer.reset();
tnhnrl 58:94b7fd55185e 481 _isTimeoutRunning = false;
tnhnrl 58:94b7fd55185e 482 }
tnhnrl 58:94b7fd55185e 483 else if (depthLoop().getPosition() < 0.5) {
tnhnrl 58:94b7fd55185e 484 pc().printf("PR: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 58:94b7fd55185e 485 _state = FLOAT_BROADCAST;
tnhnrl 58:94b7fd55185e 486 timer.reset();
tnhnrl 58:94b7fd55185e 487 _isTimeoutRunning = false;
tnhnrl 58:94b7fd55185e 488 }
tnhnrl 58:94b7fd55185e 489
tnhnrl 58:94b7fd55185e 490 // what is active?
tnhnrl 58:94b7fd55185e 491 pc().printf("PR: bce pos (cmd pos): %3.1f mm (%0.1f), batt pos: %3.1f mm (%0.1f), rudder: %f (depth: %3.1f ft, pitch: %3.1f, heading: %3.1f [cmd: %0.1f])[%0.2f sec] [imu heading: %0.1f] \r", bce().getPosition_mm(),bce().getSetPosition_mm(), batt().getPosition_mm(),batt().getSetPosition_mm(), rudder().getPosition_pwm(), depthLoop().getPosition(), pitchLoop().getPosition(), headingLoop().getPosition(), headingLoop().getOutput(), timer.read(), imu().getHeading());
tnhnrl 58:94b7fd55185e 492
tnhnrl 58:94b7fd55185e 493 // ACTIVE RUDDER CONTROL
tnhnrl 58:94b7fd55185e 494 rudder().setPosition_deg(headingLoop().getOutput());
tnhnrl 58:94b7fd55185e 495
tnhnrl 58:94b7fd55185e 496 //record data internally (for access by MbedLogger)
tnhnrl 58:94b7fd55185e 497 recordData(_state);
tnhnrl 58:94b7fd55185e 498
tnhnrl 58:94b7fd55185e 499 break;
tnhnrl 58:94b7fd55185e 500 // NEW DIVE AND RISE SEQUENCES
tnhnrl 16:3363b9f14913 501
tnhnrl 16:3363b9f14913 502 case FLOAT_LEVEL :
tnhnrl 16:3363b9f14913 503 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 504 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 505 pc().printf("\r\n\nstate: FLOAT_LEVEL\r\n");
tnhnrl 16:3363b9f14913 506 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 507 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 508 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 509
tnhnrl 16:3363b9f14913 510 // what needs to be started?
tnhnrl 16:3363b9f14913 511 bce().unpause();
tnhnrl 16:3363b9f14913 512 batt().unpause();
tnhnrl 16:3363b9f14913 513
tnhnrl 20:8987a9ae2bc7 514 // what are the commands?
tnhnrl 28:16c83a2fdefa 515 bce().setPosition_mm(_bceFloatPosition);
tnhnrl 16:3363b9f14913 516 pitchLoop().setCommand(0.0);
tnhnrl 32:f2f8ae34aadc 517
tnhnrl 32:f2f8ae34aadc 518 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 519 //createNewFile();
tnhnrl 32:f2f8ae34aadc 520
tnhnrl 32:f2f8ae34aadc 521 //show that this is the start of a new float level sequence
tnhnrl 32:f2f8ae34aadc 522 recordState(_state);
tnhnrl 32:f2f8ae34aadc 523
tnhnrl 49:47ffa4feb6db 524 //triggers logger array
tnhnrl 32:f2f8ae34aadc 525 _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 526 recordData(_state);
tnhnrl 16:3363b9f14913 527 }
tnhnrl 20:8987a9ae2bc7 528
tnhnrl 16:3363b9f14913 529 // how exit?
tnhnrl 17:7c16b5671d0e 530 if (timer > _timeout) {
tnhnrl 16:3363b9f14913 531 pc().printf("FL: timed out\r\n");
tnhnrl 21:38c8544db6f4 532 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 533 timer.reset();
tnhnrl 28:16c83a2fdefa 534 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 535 }
tnhnrl 28:16c83a2fdefa 536 else if (fabs(imu().getPitch() - pitchLoop().getCommand()) < fabs(_pitchTolerance)) { //current tolerance is 5 degrees
tnhnrl 28:16c83a2fdefa 537 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 538 _state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 539 timer.reset();
tnhnrl 28:16c83a2fdefa 540 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 541 }
tnhnrl 20:8987a9ae2bc7 542
tnhnrl 16:3363b9f14913 543 // what is active?
tnhnrl 16:3363b9f14913 544 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 545 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 546
tnhnrl 32:f2f8ae34aadc 547 //record data every 5 seconds
tnhnrl 34:9b66c5188051 548 recordData(_state);
tnhnrl 32:f2f8ae34aadc 549
tnhnrl 16:3363b9f14913 550 break;
tnhnrl 16:3363b9f14913 551
tnhnrl 16:3363b9f14913 552 case FLOAT_BROADCAST :
tnhnrl 16:3363b9f14913 553 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 554 if (!_isTimeoutRunning) {
tnhnrl 16:3363b9f14913 555 pc().printf("\r\n\nstate: FLOAT_BROADCAST\r\n");
tnhnrl 16:3363b9f14913 556 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 557 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 558 _isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 559
tnhnrl 16:3363b9f14913 560 // what needs to be started?
tnhnrl 16:3363b9f14913 561 bce().unpause();
tnhnrl 16:3363b9f14913 562 batt().unpause();
tnhnrl 20:8987a9ae2bc7 563
tnhnrl 16:3363b9f14913 564 // what are the commands?
tnhnrl 55:f4ec445c42fe 565 bce().setPosition_mm(_bceFloatPosition); // 320.0
tnhnrl 55:f4ec445c42fe 566 batt().setPosition_mm(_battFloatPosition); // 73.0
tnhnrl 32:f2f8ae34aadc 567
tnhnrl 32:f2f8ae34aadc 568 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 569 //createNewFile();
tnhnrl 32:f2f8ae34aadc 570
tnhnrl 32:f2f8ae34aadc 571 //show that this is the start of a new float broadcast sequence
tnhnrl 32:f2f8ae34aadc 572 recordState(_state);
tnhnrl 32:f2f8ae34aadc 573
tnhnrl 55:f4ec445c42fe 574 //triggers logger arrayc
tnhnrl 32:f2f8ae34aadc 575 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 576 recordData(_state);
tnhnrl 16:3363b9f14913 577 }
tnhnrl 20:8987a9ae2bc7 578
tnhnrl 16:3363b9f14913 579 // how exit?
tnhnrl 17:7c16b5671d0e 580 if (timer > _timeout) {
tnhnrl 16:3363b9f14913 581 pc().printf("FB: timed out\r\n");
tnhnrl 21:38c8544db6f4 582 _state = SIT_IDLE;
tnhnrl 16:3363b9f14913 583 timer.reset();
tnhnrl 32:f2f8ae34aadc 584
tnhnrl 32:f2f8ae34aadc 585 //stop recording data
tnhnrl 35:2f66ea4863d5 586 //mbedLogger().closeFile();
tnhnrl 32:f2f8ae34aadc 587
tnhnrl 28:16c83a2fdefa 588 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 589 }
tnhnrl 20:8987a9ae2bc7 590 else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
tnhnrl 20:8987a9ae2bc7 591 (fabs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) {
tnhnrl 16:3363b9f14913 592 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 593 _state = SIT_IDLE;
tnhnrl 16:3363b9f14913 594 timer.reset();
tnhnrl 32:f2f8ae34aadc 595
tnhnrl 32:f2f8ae34aadc 596 //stop recording data
tnhnrl 35:2f66ea4863d5 597 //mbedLogger().closeFile();
tnhnrl 32:f2f8ae34aadc 598
tnhnrl 28:16c83a2fdefa 599 _isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 600 }
tnhnrl 20:8987a9ae2bc7 601
tnhnrl 20:8987a9ae2bc7 602 // what is active?
tnhnrl 58:94b7fd55185e 603 pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm, heading(IMU): %0.1f (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), imu().getHeading(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
tnhnrl 32:f2f8ae34aadc 604
tnhnrl 32:f2f8ae34aadc 605 //record data every 5 seconds
tnhnrl 34:9b66c5188051 606 recordData(_state);
tnhnrl 32:f2f8ae34aadc 607
tnhnrl 16:3363b9f14913 608 break;
tnhnrl 17:7c16b5671d0e 609
tnhnrl 17:7c16b5671d0e 610 case MULTI_DIVE :
tnhnrl 17:7c16b5671d0e 611 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 612 if (!_isTimeoutRunning) {
tnhnrl 17:7c16b5671d0e 613 pc().printf("\r\n\nstate: MULTI-DIVE\r\n");
tnhnrl 17:7c16b5671d0e 614 timer.reset(); // timer goes back to zero
tnhnrl 17:7c16b5671d0e 615 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 616 _isTimeoutRunning = true;
tnhnrl 17:7c16b5671d0e 617
tnhnrl 17:7c16b5671d0e 618 // what needs to be started?
tnhnrl 17:7c16b5671d0e 619 bce().unpause();
tnhnrl 17:7c16b5671d0e 620 batt().unpause();
tnhnrl 17:7c16b5671d0e 621
tnhnrl 21:38c8544db6f4 622 //retrieve commands from structs (loaded from sequence.cfg file)
tnhnrl 32:f2f8ae34aadc 623 float sequence_depth_command = currentStateStruct.depth;
tnhnrl 32:f2f8ae34aadc 624 float sequence_pitch_command = currentStateStruct.pitch;
tnhnrl 17:7c16b5671d0e 625
tnhnrl 17:7c16b5671d0e 626 // what are the commands?
tnhnrl 32:f2f8ae34aadc 627 depthLoop().setCommand(sequence_depth_command);
tnhnrl 32:f2f8ae34aadc 628 pitchLoop().setCommand(sequence_pitch_command);
tnhnrl 21:38c8544db6f4 629 pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 630
tnhnrl 32:f2f8ae34aadc 631 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 632 //createNewFile();
tnhnrl 32:f2f8ae34aadc 633
tnhnrl 32:f2f8ae34aadc 634 //show that this is the start of a new MULTI_DIVE sequence
tnhnrl 32:f2f8ae34aadc 635 recordState(_state);
tnhnrl 32:f2f8ae34aadc 636
tnhnrl 32:f2f8ae34aadc 637 //no max depth recording right now
tnhnrl 32:f2f8ae34aadc 638
tnhnrl 49:47ffa4feb6db 639 //triggers logger array
tnhnrl 32:f2f8ae34aadc 640 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 641 recordData(_state);
tnhnrl 17:7c16b5671d0e 642 }
tnhnrl 20:8987a9ae2bc7 643
tnhnrl 17:7c16b5671d0e 644 // how exit?
tnhnrl 17:7c16b5671d0e 645 if (timer > _timeout) {
tnhnrl 58:94b7fd55185e 646 pc().printf("\r\n\nMULTI-DIVE: timed out [time: %0.1f]\r\n\n", timer.read());
tnhnrl 21:38c8544db6f4 647 _state = MULTI_RISE; //new behavior 11/17/2017
tnhnrl 17:7c16b5671d0e 648 timer.reset();
tnhnrl 28:16c83a2fdefa 649 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 650 }
tnhnrl 17:7c16b5671d0e 651 else if (depthLoop().getPosition() > depthLoop().getCommand()) {
tnhnrl 17:7c16b5671d0e 652 pc().printf("MULTI-DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 21:38c8544db6f4 653 _state = MULTI_RISE;
tnhnrl 17:7c16b5671d0e 654 timer.reset();
tnhnrl 28:16c83a2fdefa 655 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 656 }
tnhnrl 20:8987a9ae2bc7 657
tnhnrl 17:7c16b5671d0e 658 // what is active?
tnhnrl 17:7c16b5671d0e 659 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 660 bce().setPosition_mm(depthLoop().getOutput());
tnhnrl 17:7c16b5671d0e 661 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 662
tnhnrl 32:f2f8ae34aadc 663 //record data every 5 seconds
tnhnrl 34:9b66c5188051 664 recordData(_state);
tnhnrl 32:f2f8ae34aadc 665
tnhnrl 17:7c16b5671d0e 666 break;
tnhnrl 17:7c16b5671d0e 667
tnhnrl 17:7c16b5671d0e 668 case MULTI_RISE :
tnhnrl 17:7c16b5671d0e 669 // start local state timer and init any other one-shot actions
tnhnrl 28:16c83a2fdefa 670 if (!_isTimeoutRunning) {
tnhnrl 17:7c16b5671d0e 671 pc().printf("\r\n\nstate: MULTI-RISE\r\n");
tnhnrl 17:7c16b5671d0e 672 timer.reset(); // timer goes back to zero
tnhnrl 17:7c16b5671d0e 673 timer.start(); // background timer starts running
tnhnrl 28:16c83a2fdefa 674 _isTimeoutRunning = true;
tnhnrl 17:7c16b5671d0e 675
tnhnrl 17:7c16b5671d0e 676 // what needs to be started?
tnhnrl 17:7c16b5671d0e 677 bce().unpause();
tnhnrl 17:7c16b5671d0e 678 batt().unpause();
tnhnrl 17:7c16b5671d0e 679
tnhnrl 17:7c16b5671d0e 680 //NEW: retrieve depth and pitch commands from config file struct
tnhnrl 17:7c16b5671d0e 681 // concept is to load this each time the multi-dive restarts
tnhnrl 17:7c16b5671d0e 682 stateMachine().getDiveSequence();
tnhnrl 17:7c16b5671d0e 683
tnhnrl 17:7c16b5671d0e 684 //retrieve just pitch command from struct
tnhnrl 32:f2f8ae34aadc 685 float sequence_pitch_command = currentStateStruct.pitch;
tnhnrl 17:7c16b5671d0e 686
tnhnrl 17:7c16b5671d0e 687 // what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017
tnhnrl 17:7c16b5671d0e 688 depthLoop().setCommand(0.5);
tnhnrl 32:f2f8ae34aadc 689 pitchLoop().setCommand(-sequence_pitch_command);
tnhnrl 21:38c8544db6f4 690 pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
tnhnrl 32:f2f8ae34aadc 691
tnhnrl 32:f2f8ae34aadc 692 //create the log file (works only if the file is closed)
tnhnrl 34:9b66c5188051 693 //createNewFile();
tnhnrl 32:f2f8ae34aadc 694
tnhnrl 32:f2f8ae34aadc 695 //show that this is the start of a new MULTI_DIVE sequence
tnhnrl 32:f2f8ae34aadc 696 recordState(_state);
tnhnrl 32:f2f8ae34aadc 697
tnhnrl 49:47ffa4feb6db 698 //triggers logger array
tnhnrl 32:f2f8ae34aadc 699 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 700 recordData(_state);
tnhnrl 17:7c16b5671d0e 701 }
tnhnrl 20:8987a9ae2bc7 702
tnhnrl 17:7c16b5671d0e 703 // how exit?
tnhnrl 17:7c16b5671d0e 704 if (timer > _timeout) {
tnhnrl 58:94b7fd55185e 705 pc().printf("MULTI-RISE: timed out [time: %0.1f]\r\n\n", timer.read());
tnhnrl 21:38c8544db6f4 706 _state = EMERGENCY_CLIMB;
tnhnrl 17:7c16b5671d0e 707 timer.reset();
tnhnrl 28:16c83a2fdefa 708 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 709
tnhnrl 17:7c16b5671d0e 710 //reset multi-dive sequence to start
tnhnrl 24:c7d9b5bf3829 711 _multi_dive_counter = 0;
tnhnrl 63:6cb0405fc6e6 712
tnhnrl 63:6cb0405fc6e6 713 //Reload the dive sequence on exit
tnhnrl 63:6cb0405fc6e6 714 sequenceController().loadSequence();
tnhnrl 17:7c16b5671d0e 715 }
tnhnrl 20:8987a9ae2bc7 716 else if (depthLoop().getPosition() < 0.5) { // depth is less than 0.5 (zero is surface level)
tnhnrl 17:7c16b5671d0e 717 pc().printf("MULTI-RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 17:7c16b5671d0e 718
tnhnrl 17:7c16b5671d0e 719 //going to next state
tnhnrl 28:16c83a2fdefa 720 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 721
tnhnrl 17:7c16b5671d0e 722 //successful dive-rise sequence CONTINUES the multi-dive sequence
tnhnrl 24:c7d9b5bf3829 723 _multi_dive_counter++;
tnhnrl 17:7c16b5671d0e 724
tnhnrl 17:7c16b5671d0e 725 //UPDATE THE SEQUENCE DATA HERE
tnhnrl 17:7c16b5671d0e 726 stateMachine().getDiveSequence();
tnhnrl 17:7c16b5671d0e 727
tnhnrl 17:7c16b5671d0e 728 //check if this is the end of the dive sequence
tnhnrl 30:2964617e7676 729 //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (FLOAT_BROADCAST)
tnhnrl 30:2964617e7676 730 if (currentStateStruct.state == FLOAT_BROADCAST) {
tnhnrl 63:6cb0405fc6e6 731 //Reload the dive sequence on exit
tnhnrl 63:6cb0405fc6e6 732 sequenceController().loadSequence();
tnhnrl 63:6cb0405fc6e6 733
tnhnrl 28:16c83a2fdefa 734 _state = FLOAT_BROADCAST;
tnhnrl 17:7c16b5671d0e 735 }
tnhnrl 17:7c16b5671d0e 736
tnhnrl 17:7c16b5671d0e 737 else
tnhnrl 63:6cb0405fc6e6 738 _state = MULTI_DIVE; //Note: need to test if this else statement is necessary
tnhnrl 17:7c16b5671d0e 739
tnhnrl 24:c7d9b5bf3829 740 //have to stop this with the _multi_dive_counter variable!
tnhnrl 17:7c16b5671d0e 741 }
tnhnrl 20:8987a9ae2bc7 742
tnhnrl 20:8987a9ae2bc7 743 // what is active?
tnhnrl 17:7c16b5671d0e 744 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 745 bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
tnhnrl 17:7c16b5671d0e 746 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 32:f2f8ae34aadc 747
tnhnrl 32:f2f8ae34aadc 748 //record data every 5 seconds
tnhnrl 34:9b66c5188051 749 recordData(_state);
tnhnrl 32:f2f8ae34aadc 750
tnhnrl 17:7c16b5671d0e 751 break;
tnhnrl 32:f2f8ae34aadc 752
tnhnrl 63:6cb0405fc6e6 753 case TRANSMIT_MBED_LOG:
tnhnrl 63:6cb0405fc6e6 754 if (!_isTimeoutRunning) {
tnhnrl 63:6cb0405fc6e6 755 pc().printf("\r\n\nstate: TRANSMIT_MBED_LOG\r\n");
tnhnrl 32:f2f8ae34aadc 756 timer.reset(); // timer goes back to zero
tnhnrl 32:f2f8ae34aadc 757 timer.start(); // background timer starts running
tnhnrl 32:f2f8ae34aadc 758 _isTimeoutRunning = true;
tnhnrl 32:f2f8ae34aadc 759
tnhnrl 45:16b8162188ca 760 mbedLogger().getNumberOfPacketsInCurrentLog(); //open the file, read the number of lines in the log file
tnhnrl 45:16b8162188ca 761 transmit_packet_number = mbedLogger().getNumberOfPackets();
tnhnrl 45:16b8162188ca 762
tnhnrl 63:6cb0405fc6e6 763 //set the timeout internally (integer value)!
tnhnrl 63:6cb0405fc6e6 764 _timeout = mbedLogger().getLogSize(); //works after current log function above
tnhnrl 63:6cb0405fc6e6 765 _timeout = _timeout - (_timeout % 10) + 10; //remove the last digit and keep it as multiples of 10 (add a buffer of 10 seconds just in case it's below 10)
tnhnrl 63:6cb0405fc6e6 766
tnhnrl 63:6cb0405fc6e6 767 //STATE
tnhnrl 63:6cb0405fc6e6 768 //TRANSMIT_MBED_LOG
tnhnrl 45:16b8162188ca 769 }
tnhnrl 45:16b8162188ca 770
tnhnrl 45:16b8162188ca 771 if (timer.read() > _timeout) {
tnhnrl 63:6cb0405fc6e6 772 mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
tnhnrl 63:6cb0405fc6e6 773 pc().printf("\r\TRANSMIT_MBED_LOG: timed out!\r\n");
tnhnrl 63:6cb0405fc6e6 774
tnhnrl 63:6cb0405fc6e6 775 //STATE
tnhnrl 45:16b8162188ca 776 _state = SIT_IDLE;
tnhnrl 63:6cb0405fc6e6 777
tnhnrl 63:6cb0405fc6e6 778 timer.reset();
tnhnrl 63:6cb0405fc6e6 779 _isTimeoutRunning = false;
tnhnrl 63:6cb0405fc6e6 780
tnhnrl 63:6cb0405fc6e6 781 //send a command to the Python GUI to stop looking for data
tnhnrl 63:6cb0405fc6e6 782 //SEND COMMAND WHEN TRANSMISSION ENDS PREMATURELY
tnhnrl 63:6cb0405fc6e6 783 pc().printf("%c%c%c%c",0x15,0x15,0x15,0x15);
tnhnrl 63:6cb0405fc6e6 784 }
tnhnrl 63:6cb0405fc6e6 785
tnhnrl 63:6cb0405fc6e6 786 //IF THIS IS ZERO
tnhnrl 63:6cb0405fc6e6 787 if (mbedLogger().currentPacketNumber() > transmit_packet_number) {
tnhnrl 63:6cb0405fc6e6 788 pc().printf("mbedLogger().currentPacketNumber() > transmit_packet_number");
tnhnrl 63:6cb0405fc6e6 789
tnhnrl 63:6cb0405fc6e6 790 //STATE
tnhnrl 63:6cb0405fc6e6 791 _state = SIT_IDLE;
tnhnrl 63:6cb0405fc6e6 792
tnhnrl 45:16b8162188ca 793 timer.reset();
tnhnrl 45:16b8162188ca 794 _isTimeoutRunning = false;
tnhnrl 45:16b8162188ca 795
tnhnrl 45:16b8162188ca 796 mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
tnhnrl 45:16b8162188ca 797 }
tnhnrl 45:16b8162188ca 798
tnhnrl 63:6cb0405fc6e6 799 //What is active?
tnhnrl 45:16b8162188ca 800 mbedLogger().readTransmitPacketOneChar(); //led2 shows you pc readable
tnhnrl 45:16b8162188ca 801
tnhnrl 45:16b8162188ca 802 break;
tnhnrl 45:16b8162188ca 803
tnhnrl 45:16b8162188ca 804 case RECEIVE_SEQUENCE :
tnhnrl 58:94b7fd55185e 805 pc().printf("state: RECEIVE_SEQUENCE\r\n");
tnhnrl 45:16b8162188ca 806
tnhnrl 45:16b8162188ca 807 if (!_isTimeoutRunning) {
tnhnrl 45:16b8162188ca 808 pc().printf("RECEIVE_SEQUENCE _isTimeoutRunning\r\n");
tnhnrl 45:16b8162188ca 809 timer.reset(); // timer goes back to zero
tnhnrl 45:16b8162188ca 810 timer.start(); // background timer starts running
tnhnrl 45:16b8162188ca 811 _isTimeoutRunning = true;
tnhnrl 45:16b8162188ca 812 }
tnhnrl 45:16b8162188ca 813
tnhnrl 34:9b66c5188051 814 if (timer.read() > _timeout) {
tnhnrl 58:94b7fd55185e 815 pc().printf("RECEIVE_SEQUENCE: timed out!\r\n");
tnhnrl 34:9b66c5188051 816 _state = SIT_IDLE;
tnhnrl 34:9b66c5188051 817 timer.reset();
tnhnrl 32:f2f8ae34aadc 818 _isTimeoutRunning = false;
tnhnrl 32:f2f8ae34aadc 819 }
tnhnrl 32:f2f8ae34aadc 820
tnhnrl 45:16b8162188ca 821 // what is active?
tnhnrl 58:94b7fd55185e 822 pc().printf("Receive sequence active?\r\n");
tnhnrl 32:f2f8ae34aadc 823
tnhnrl 32:f2f8ae34aadc 824 break;
tnhnrl 16:3363b9f14913 825
tnhnrl 16:3363b9f14913 826 default :
tnhnrl 58:94b7fd55185e 827 pc().printf("DEBUG: SIT_IDLE\r\n");
tnhnrl 21:38c8544db6f4 828 _state = SIT_IDLE;
tnhnrl 28:16c83a2fdefa 829 }
tnhnrl 28:16c83a2fdefa 830
tnhnrl 28:16c83a2fdefa 831 //save the state to print to user
tnhnrl 28:16c83a2fdefa 832 if (_previous_state != _state) {
tnhnrl 28:16c83a2fdefa 833 _state_array[_state_array_counter] = _state; //save to state array
tnhnrl 28:16c83a2fdefa 834 _state_array_counter++;
tnhnrl 28:16c83a2fdefa 835
tnhnrl 28:16c83a2fdefa 836 _previous_state = _state;
tnhnrl 28:16c83a2fdefa 837 }
tnhnrl 34:9b66c5188051 838
tnhnrl 34:9b66c5188051 839 return _state;
tnhnrl 34:9b66c5188051 840 // //if the state is SIT_IDLE, return 0 / false (for recording)
tnhnrl 34:9b66c5188051 841 // if (_state == SIT_IDLE)
tnhnrl 34:9b66c5188051 842 // return 0;
tnhnrl 34:9b66c5188051 843 // else
tnhnrl 34:9b66c5188051 844 // return 1; //return true to indicate that you're recording
tnhnrl 16:3363b9f14913 845 }
tnhnrl 20:8987a9ae2bc7 846
tnhnrl 16:3363b9f14913 847 // output the keyboard menu for user's reference
tnhnrl 58:94b7fd55185e 848 void StateMachine::showSimpleMenu() {
tnhnrl 58:94b7fd55185e 849 pc().printf("\r\r\n\nSIMPLE KEYBOARD MENU (06/15/2018):\r\r\n"); //make sure depth sensor tares itself on startup
tnhnrl 63:6cb0405fc6e6 850 pc().printf(" Neutral Positions BCE: %0.1f BMM: %0.1f \r\n", _neutral_bce_pos_mm, _neutral_batt_pos_mm);
tnhnrl 58:94b7fd55185e 851
tnhnrl 58:94b7fd55185e 852 pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
tnhnrl 57:ec69651c8c21 853 pc().printf(" J to float level\r\n");
tnhnrl 57:ec69651c8c21 854 pc().printf(" B to float at broadcast pitch\r\n");
tnhnrl 57:ec69651c8c21 855 pc().printf(" E to initiate emergency climb\r\n");
tnhnrl 57:ec69651c8c21 856 pc().printf(" P to print the current log file.\r\n");
tnhnrl 57:ec69651c8c21 857 pc().printf(" O to transmit current log file.\r\n");
tnhnrl 57:ec69651c8c21 858 pc().printf(" ~ to erase mbed log file. (clear before logging more than a few runs)\r\n");
tnhnrl 58:94b7fd55185e 859
tnhnrl 58:94b7fd55185e 860 pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
tnhnrl 58:94b7fd55185e 861 pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);
tnhnrl 58:94b7fd55185e 862 pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
tnhnrl 58:94b7fd55185e 863
tnhnrl 58:94b7fd55185e 864 pc().printf("-/+ to decrease/increase timeout: %d s\r\n",_timeout);
tnhnrl 57:ec69651c8c21 865 pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
tnhnrl 58:94b7fd55185e 866 pc().printf(" 8 STREAM SENSOR STATUS (and channel readings)\r\n");
tnhnrl 58:94b7fd55185e 867
tnhnrl 57:ec69651c8c21 868 pc().printf(" ? to reset mbed\r\n");
tnhnrl 58:94b7fd55185e 869 pc().printf(" * (asterisk) to go to DEBUG keyboard menu\r\n");
tnhnrl 57:ec69651c8c21 870 }
tnhnrl 57:ec69651c8c21 871
tnhnrl 57:ec69651c8c21 872 void StateMachine::showDebugMenu() {
tnhnrl 58:94b7fd55185e 873 pc().printf("\r\r\n\nDEBUG KEYBOARD MENU (06/15/2018):\r\r\n");
tnhnrl 58:94b7fd55185e 874 pc().printf(" T to go into CHECK NEUTRAL TUNING (This is on a timer! Uses NEUTRAL positions!)\r\n");
tnhnrl 58:94b7fd55185e 875 pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
tnhnrl 16:3363b9f14913 876 pc().printf(" N to find neutral\r\n");
tnhnrl 17:7c16b5671d0e 877 pc().printf(" M to initiate multi-dive cycle\r\n");
tnhnrl 16:3363b9f14913 878 pc().printf(" D to initiate dive cycle\r\n");
tnhnrl 16:3363b9f14913 879 pc().printf(" R to initiate rise\r\n");
tnhnrl 57:ec69651c8c21 880 pc().printf(" J to float level\r\n");
tnhnrl 16:3363b9f14913 881 pc().printf(" B to float at broadcast pitch\r\n");
tnhnrl 16:3363b9f14913 882 pc().printf(" E to initiate emergency climb\r\n");
tnhnrl 49:47ffa4feb6db 883 pc().printf(" '|' to tare the depth sensor (vertical bar)\r\n");
tnhnrl 28:16c83a2fdefa 884 pc().printf(" Z to show FSM and sub-FSM states.\r\n");
tnhnrl 32:f2f8ae34aadc 885 pc().printf(" P to print the current log file.\r\n");
tnhnrl 32:f2f8ae34aadc 886 pc().printf(" X to print the list of log files.\r\n");
tnhnrl 49:47ffa4feb6db 887 pc().printf(" I to receive data.\r\n");
tnhnrl 49:47ffa4feb6db 888 pc().printf(" O to transmit data.\r\n");
tnhnrl 49:47ffa4feb6db 889 pc().printf(" ~ to erase mbed log file. (clear before logging more than a few runs)\r\n");
tnhnrl 32:f2f8ae34aadc 890 pc().printf("[/] to change bce neutral position: %0.1f\r\n", _neutral_bce_pos_mm);
tnhnrl 32:f2f8ae34aadc 891 pc().printf("</> to change batt neutral position: %0.1f\r\n", _neutral_batt_pos_mm);
tnhnrl 32:f2f8ae34aadc 892 pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitch_command);
tnhnrl 32:f2f8ae34aadc 893 pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depth_command);
tnhnrl 58:94b7fd55185e 894 pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
tnhnrl 58:94b7fd55185e 895 pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
tnhnrl 58:94b7fd55185e 896 pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);
tnhnrl 58:94b7fd55185e 897 pc().printf("-/+ to decrease/increase timeout: %d s\r\n",_timeout);
tnhnrl 58:94b7fd55185e 898
tnhnrl 58:94b7fd55185e 899 // FIXED WITH NEW CODE
tnhnrl 16:3363b9f14913 900 pc().printf(" 1 BCE PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 901 pc().printf(" 2 BATT PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 902 pc().printf(" 3 Depth PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 903 pc().printf(" 4 Pitch PID sub-menu\r\n");
tnhnrl 58:94b7fd55185e 904 pc().printf(" 5 Rudder (servo) sub-menu\r\n");
tnhnrl 58:94b7fd55185e 905 pc().printf(" 6 HEADING PID sub-menu\r\n");
tnhnrl 58:94b7fd55185e 906 pc().printf(" 7 MANUAL_TUNING sub-menu (does not have a timer!) *** MOTORS ARE ACTIVE ***\r\n");
tnhnrl 58:94b7fd55185e 907 //pc().printf(" 8 VIEW_OUTPUTS sub-menu (does not a have a timer!)\r\n");
tnhnrl 58:94b7fd55185e 908 pc().printf(" 8 STREAM SENSOR STATUS (and channel readings)\r\n");
tnhnrl 53:c0586fe62b01 909
tnhnrl 58:94b7fd55185e 910 // FIXED WITH NEW CODE
tnhnrl 58:94b7fd55185e 911
tnhnrl 53:c0586fe62b01 912
tnhnrl 28:16c83a2fdefa 913 pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
tnhnrl 16:3363b9f14913 914 pc().printf(" ? to reset mbed\r\n");
tnhnrl 58:94b7fd55185e 915 pc().printf(" * (asterisk) to go to SIMPLE keyboard menu\r\n");
tnhnrl 16:3363b9f14913 916 }
tnhnrl 20:8987a9ae2bc7 917
tnhnrl 17:7c16b5671d0e 918 //Find Neutral sub finite state machine
tnhnrl 17:7c16b5671d0e 919 // Note: the sub-fsm only moves the pistons once at the start of each timer loop
tnhnrl 17:7c16b5671d0e 920 // (timer completes, move piston, timer completes, move piston, etc)
tnhnrl 28:16c83a2fdefa 921 int StateMachine::runNeutralStateMachine() {
tnhnrl 24:c7d9b5bf3829 922 switch (_substate) {
tnhnrl 20:8987a9ae2bc7 923 case NEUTRAL_SINKING :
tnhnrl 17:7c16b5671d0e 924 //start the 10 second timer
tnhnrl 28:16c83a2fdefa 925 if (!_isSubStateTimerRunning) {
tnhnrl 24:c7d9b5bf3829 926 _neutral_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds
tnhnrl 17:7c16b5671d0e 927
tnhnrl 58:94b7fd55185e 928 pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (getSetPosition: %0.1f)\r\n", _neutral_timer, timer.read(), pitchLoop().getPosition(), bce().getSetPosition_mm());
tnhnrl 20:8987a9ae2bc7 929
tnhnrl 32:f2f8ae34aadc 930 // what are the commands? (BCE linear actuator active, no pitch movement)
tnhnrl 39:58375ca6b6ff 931 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 932
tnhnrl 58:94b7fd55185e 933 pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\r\n", _neutral_sink_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition());
tnhnrl 17:7c16b5671d0e 934
tnhnrl 28:16c83a2fdefa 935 _isSubStateTimerRunning = true; //disable this block after one iteration
tnhnrl 17:7c16b5671d0e 936 }
tnhnrl 20:8987a9ae2bc7 937
tnhnrl 20:8987a9ae2bc7 938 // how exit?
tnhnrl 20:8987a9ae2bc7 939 //once reached the travel limit, no need to keep trying, so exit
tnhnrl 25:249e4d56b27c 940 if (bce().getPosition_mm() <= 0) {
tnhnrl 58:94b7fd55185e 941 pc().printf("\r\nDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\r\n", bce().getPosition_mm());
tnhnrl 25:249e4d56b27c 942 _substate = NEUTRAL_EXIT;
tnhnrl 28:16c83a2fdefa 943 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 25:249e4d56b27c 944 }
tnhnrl 20:8987a9ae2bc7 945 //once deeper than the commanded setpoint...
tnhnrl 32:f2f8ae34aadc 946 else if (depthLoop().getPosition() > _depth_command) {
tnhnrl 24:c7d9b5bf3829 947 _substate = NEUTRAL_SLOWLY_RISE; // next state
tnhnrl 28:16c83a2fdefa 948 _isSubStateTimerRunning = false; //reset the sub state timer
tnhnrl 20:8987a9ae2bc7 949 }
tnhnrl 20:8987a9ae2bc7 950
tnhnrl 20:8987a9ae2bc7 951 // what is active?
tnhnrl 20:8987a9ae2bc7 952 //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint
tnhnrl 24:c7d9b5bf3829 953 if (timer.read() >= _neutral_timer) {
tnhnrl 32:f2f8ae34aadc 954 pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! [current time: %0.1f]\r\n", timer.read());
tnhnrl 17:7c16b5671d0e 955
tnhnrl 28:16c83a2fdefa 956 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
tnhnrl 17:7c16b5671d0e 957 }
tnhnrl 39:58375ca6b6ff 958
tnhnrl 39:58375ca6b6ff 959 // what is active? (only the buoyancy engine moved every 5 seconds at start)
tnhnrl 39:58375ca6b6ff 960 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 961 break;
tnhnrl 17:7c16b5671d0e 962
tnhnrl 17:7c16b5671d0e 963 case NEUTRAL_SLOWLY_RISE:
tnhnrl 28:16c83a2fdefa 964 if (!_isSubStateTimerRunning) {
tnhnrl 24:c7d9b5bf3829 965 _neutral_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds
tnhnrl 17:7c16b5671d0e 966
tnhnrl 24:c7d9b5bf3829 967 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 968
tnhnrl 20:8987a9ae2bc7 969 // what are the commands?
tnhnrl 32:f2f8ae34aadc 970 //move piston at start of sequence (default: extend 2.0 mm)
tnhnrl 39:58375ca6b6ff 971 bce().setPosition_mm(bce().getSetPosition_mm() + 2.0); //no depth command
tnhnrl 23:434f04ef1fad 972
tnhnrl 23:434f04ef1fad 973 // it's okay to run the pitch outer loop now since we've already found pitch level in the previous state
tnhnrl 23:434f04ef1fad 974 pitchLoop().setCommand(0.0);
tnhnrl 24:c7d9b5bf3829 975
tnhnrl 58:94b7fd55185e 976 pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\r\n", _neutral_rise_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand());
tnhnrl 24:c7d9b5bf3829 977
tnhnrl 28:16c83a2fdefa 978 _isSubStateTimerRunning = true; //disable this block after one iteration
tnhnrl 17:7c16b5671d0e 979 }
tnhnrl 17:7c16b5671d0e 980
tnhnrl 20:8987a9ae2bc7 981 // how exit?
tnhnrl 24:c7d9b5bf3829 982 //once at full travel limit (setPosition) and haven't yet risen, time to give up and exit
tnhnrl 24:c7d9b5bf3829 983 if (bce().getSetPosition_mm() >= bce().getTravelLimit()) {
tnhnrl 24:c7d9b5bf3829 984 _substate = NEUTRAL_EXIT;
tnhnrl 28:16c83a2fdefa 985 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 17:7c16b5671d0e 986 }
tnhnrl 17:7c16b5671d0e 987 //depth rate or sink rate < 0 ft/s, go to the next substate the next iteration
tnhnrl 20:8987a9ae2bc7 988 else if (depthLoop().getVelocity() < 0) { //less than zero ft/s
tnhnrl 24:c7d9b5bf3829 989 pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 ft/s [time: %0.1f]\r\n", timer.read());
tnhnrl 24:c7d9b5bf3829 990 _substate = NEUTRAL_CHECK_PITCH;
tnhnrl 28:16c83a2fdefa 991 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 17:7c16b5671d0e 992 }
tnhnrl 17:7c16b5671d0e 993
tnhnrl 20:8987a9ae2bc7 994 // what is active?
tnhnrl 20:8987a9ae2bc7 995 //once 5 second timer complete, reset the timeout so the state one-shot entry will move the setpoint
tnhnrl 24:c7d9b5bf3829 996 if (timer.read() >= _neutral_timer) {
tnhnrl 32:f2f8ae34aadc 997 pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! [timer: %0.1f]\r\n", timer.read());
tnhnrl 20:8987a9ae2bc7 998
tnhnrl 28:16c83a2fdefa 999 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
tnhnrl 17:7c16b5671d0e 1000 }
tnhnrl 23:434f04ef1fad 1001
tnhnrl 32:f2f8ae34aadc 1002 // what is active? (only the buoyancy engine moved every 5 seconds)
tnhnrl 32:f2f8ae34aadc 1003 pc().printf("depthLoop getOutput: %0.1f\r", depthLoop().getOutput()); //debug
tnhnrl 32:f2f8ae34aadc 1004 bce().setPosition_mm(depthLoop().getOutput()); // (DID NOT WORK ON BENCH)
tnhnrl 17:7c16b5671d0e 1005 break;
tnhnrl 17:7c16b5671d0e 1006
danstrider 22:a10ee088403b 1007 case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired
danstrider 22:a10ee088403b 1008 // start local state timer and init any other one-shot actions
tnhnrl 23:434f04ef1fad 1009
tnhnrl 28:16c83a2fdefa 1010 if (!_isSubStateTimerRunning) {
tnhnrl 24:c7d9b5bf3829 1011 _neutral_timer = timer.read() + 10; // record time when this block is entered and add several seconds
tnhnrl 24:c7d9b5bf3829 1012 pc().printf("\r\nNEUTRAL_CHECK_PITCH: Next move in %0.1f sec \r\n",_neutral_timer - timer.read());
danstrider 22:a10ee088403b 1013
tnhnrl 32:f2f8ae34aadc 1014 // what are the commands? (default: retract or extend 0.5 mm)
tnhnrl 24:c7d9b5bf3829 1015 if (pitchLoop().getPosition() > 2) { // nose is high
tnhnrl 39:58375ca6b6ff 1016 batt().setPosition_mm(batt().getSetPosition_mm() + 0.5); // move battery forward (using setpoint from linear actuator)
tnhnrl 58:94b7fd55185e 1017 pc().printf("\r\nNeutral Check Pitch: moving battery FWD in %0.1f mm increments\r\n\n", _neutral_pitch_command_mm);
danstrider 22:a10ee088403b 1018 }
tnhnrl 24:c7d9b5bf3829 1019 else if (pitchLoop().getPosition() < -2) { // nose is low
tnhnrl 39:58375ca6b6ff 1020 batt().setPosition_mm(batt().getSetPosition_mm() - 0.5); // move battery aft (using setpoint from linear actuator)
tnhnrl 58:94b7fd55185e 1021 pc().printf("\r\nNeutral Check Pitch: moving battery AFT in %0.1f mm increments\r\n\n", _neutral_pitch_command_mm);
danstrider 22:a10ee088403b 1022 }
tnhnrl 24:c7d9b5bf3829 1023
tnhnrl 28:16c83a2fdefa 1024 _isSubStateTimerRunning = true; //disable this block after one iteration
danstrider 22:a10ee088403b 1025 }
tnhnrl 20:8987a9ae2bc7 1026
tnhnrl 28:16c83a2fdefa 1027 // how exit?
tnhnrl 20:8987a9ae2bc7 1028 //pitch angle and pitch rate within small tolerance
tnhnrl 20:8987a9ae2bc7 1029 //benchtop tests confirm angle needs to be around 2 degrees
tnhnrl 23:434f04ef1fad 1030 if ((fabs(pitchLoop().getPosition()) < 2.0) and (fabs(pitchLoop().getVelocity()) < 5.0)) {
tnhnrl 58:94b7fd55185e 1031 pc().printf("Debug: Found Level (NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH)\r\n"); //debug
danstrider 22:a10ee088403b 1032 // found level, but don't need to save anything this time
tnhnrl 23:434f04ef1fad 1033
tnhnrl 28:16c83a2fdefa 1034 if (depthLoop().getPosition() > _max_recorded_depth_neutral) { //debug
tnhnrl 28:16c83a2fdefa 1035 _max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded
tnhnrl 28:16c83a2fdefa 1036 }
tnhnrl 28:16c83a2fdefa 1037
tnhnrl 23:434f04ef1fad 1038 // found level and at depth too, so save it all now
tnhnrl 32:f2f8ae34aadc 1039 if (_substate == NEUTRAL_CHECK_PITCH) {
danstrider 22:a10ee088403b 1040 //save positions locally
danstrider 22:a10ee088403b 1041 _neutral_batt_pos_mm = batt().getPosition_mm();
danstrider 22:a10ee088403b 1042 _neutral_bce_pos_mm = bce().getPosition_mm();
danstrider 22:a10ee088403b 1043
danstrider 22:a10ee088403b 1044 //set the neutral positions in each outer loop
danstrider 22:a10ee088403b 1045 depthLoop().setOutputOffset(_neutral_bce_pos_mm);
danstrider 22:a10ee088403b 1046 pitchLoop().setOutputOffset(_neutral_batt_pos_mm);
danstrider 22:a10ee088403b 1047
danstrider 22:a10ee088403b 1048 // save into the depth.txt and pitch.txt files
danstrider 22:a10ee088403b 1049 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset
danstrider 22:a10ee088403b 1050 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset
danstrider 22:a10ee088403b 1051
tnhnrl 58:94b7fd55185e 1052 pc().printf("\r\n\n>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\r\n\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
danstrider 22:a10ee088403b 1053
tnhnrl 24:c7d9b5bf3829 1054 _substate = NEUTRAL_EXIT;
tnhnrl 28:16c83a2fdefa 1055 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
tnhnrl 24:c7d9b5bf3829 1056 }
tnhnrl 24:c7d9b5bf3829 1057
tnhnrl 24:c7d9b5bf3829 1058 else {
tnhnrl 58:94b7fd55185e 1059 pc().printf("\r\nDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\r\n");
tnhnrl 24:c7d9b5bf3829 1060 _substate = NEUTRAL_EXIT;
danstrider 22:a10ee088403b 1061 }
tnhnrl 17:7c16b5671d0e 1062 }
danstrider 22:a10ee088403b 1063
danstrider 22:a10ee088403b 1064 // what is active?
danstrider 22:a10ee088403b 1065 //once timer complete, reset the timeout so the state one-shot entry will move the setpoint
tnhnrl 24:c7d9b5bf3829 1066 if (timer.read() >= _neutral_timer) {
danstrider 22:a10ee088403b 1067 pc().printf("\r\n\nlevel timer COMPLETE!");
danstrider 22:a10ee088403b 1068 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 1069 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
danstrider 22:a10ee088403b 1070 }
tnhnrl 17:7c16b5671d0e 1071 break;
danstrider 22:a10ee088403b 1072
danstrider 22:a10ee088403b 1073 //this state could be removed, it is only used as a transition but is needed to stop entering this function
danstrider 22:a10ee088403b 1074 case NEUTRAL_EXIT :
tnhnrl 58:94b7fd55185e 1075 pc().printf("substate: NEUTRAL_EXIT\r\n");
tnhnrl 20:8987a9ae2bc7 1076 break;
tnhnrl 21:38c8544db6f4 1077
danstrider 22:a10ee088403b 1078 default :
tnhnrl 58:94b7fd55185e 1079 pc().printf("how did we get to substate: default?\r\n"); //debug
tnhnrl 23:434f04ef1fad 1080 //a default within the sub-state machine
tnhnrl 24:c7d9b5bf3829 1081 _substate = NEUTRAL_EXIT;
danstrider 22:a10ee088403b 1082 break;
tnhnrl 17:7c16b5671d0e 1083 }
tnhnrl 20:8987a9ae2bc7 1084
tnhnrl 30:2964617e7676 1085 // reset the sub-FSM if needed (useful if you need to redo the neutral-finding sequence)
tnhnrl 24:c7d9b5bf3829 1086 if (_substate == NEUTRAL_EXIT) {
tnhnrl 58:94b7fd55185e 1087 pc().printf("******************************** EXITING sub-FSM! *******************************\r\n\n");
tnhnrl 24:c7d9b5bf3829 1088
tnhnrl 30:2964617e7676 1089 //reset internal sub-state back to first entry conditions (first state is immediately sinking)
tnhnrl 30:2964617e7676 1090 _substate = NEUTRAL_SINKING;
tnhnrl 28:16c83a2fdefa 1091 _isSubStateTimerRunning = false; // reset the sub state timer
tnhnrl 21:38c8544db6f4 1092
tnhnrl 24:c7d9b5bf3829 1093 //record sub-states to view after sequence
tnhnrl 30:2964617e7676 1094 _substate_array[_substate_array_counter] = NEUTRAL_EXIT; //save exit to state array
tnhnrl 28:16c83a2fdefa 1095 _substate_array_counter++;
tnhnrl 23:434f04ef1fad 1096
tnhnrl 24:c7d9b5bf3829 1097 //reset _previous_substate on exit (has to be done in FIND_NEUTRAL if emergency exit)
tnhnrl 24:c7d9b5bf3829 1098 _previous_substate = -1;
tnhnrl 24:c7d9b5bf3829 1099
tnhnrl 24:c7d9b5bf3829 1100 //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed
tnhnrl 24:c7d9b5bf3829 1101 return NEUTRAL_EXIT; // message to calling function we just exited
tnhnrl 21:38c8544db6f4 1102 }
tnhnrl 23:434f04ef1fad 1103 else {
tnhnrl 24:c7d9b5bf3829 1104 //record sub-states to view after sequence (when changed)
tnhnrl 24:c7d9b5bf3829 1105 if (_previous_substate != _substate) {
tnhnrl 28:16c83a2fdefa 1106 _substate_array[_substate_array_counter] = _substate; //save current state to state array
tnhnrl 28:16c83a2fdefa 1107 _substate_array_counter++;
tnhnrl 24:c7d9b5bf3829 1108
tnhnrl 24:c7d9b5bf3829 1109 //record the current substate for comparison
tnhnrl 24:c7d9b5bf3829 1110 _previous_substate = _substate;
tnhnrl 24:c7d9b5bf3829 1111 }
tnhnrl 24:c7d9b5bf3829 1112
tnhnrl 24:c7d9b5bf3829 1113 return _substate; // message to calling function of what sub-state it's in
tnhnrl 23:434f04ef1fad 1114 }
tnhnrl 17:7c16b5671d0e 1115 }
tnhnrl 20:8987a9ae2bc7 1116
tnhnrl 57:ec69651c8c21 1117 /* keyboard runs independently of the state machine, handling one key at a time
tnhnrl 57:ec69651c8c21 1118 keyboard updates the desired _keyboard_state that is used in the state machine
tnhnrl 57:ec69651c8c21 1119 and only allows input when the state is "idle" */
tnhnrl 57:ec69651c8c21 1120
tnhnrl 17:7c16b5671d0e 1121 void StateMachine::keyboard() {
tnhnrl 57:ec69651c8c21 1122 char user_input;
tnhnrl 20:8987a9ae2bc7 1123
tnhnrl 16:3363b9f14913 1124 // check keyboard and make settings changes as requested
tnhnrl 17:7c16b5671d0e 1125 // states can be changed only at the start of a sequence (when the system is in SIT_IDLE)
tnhnrl 21:38c8544db6f4 1126
tnhnrl 37:357e98a929cc 1127 //TEST
tnhnrl 37:357e98a929cc 1128 int _keyboard_state = 0; //made this a local variable because it was retaining the last keyboard state
tnhnrl 21:38c8544db6f4 1129
tnhnrl 28:16c83a2fdefa 1130 if (pc().readable() && (_state == SIT_IDLE || _state == KEYBOARD)) {
tnhnrl 16:3363b9f14913 1131 // get the key
tnhnrl 57:ec69651c8c21 1132 user_input = pc().getc();
tnhnrl 17:7c16b5671d0e 1133
tnhnrl 28:16c83a2fdefa 1134 //record that the keyboard was used
tnhnrl 28:16c83a2fdefa 1135 _state_array[_state_array_counter] = KEYBOARD;
tnhnrl 28:16c83a2fdefa 1136 _state_array_counter++;
tnhnrl 28:16c83a2fdefa 1137
tnhnrl 21:38c8544db6f4 1138 // keyboard has to reset timer each time it's used
tnhnrl 28:16c83a2fdefa 1139 _isTimeoutRunning = false;
tnhnrl 17:7c16b5671d0e 1140
tnhnrl 16:3363b9f14913 1141 // check command against desired control buttons
tnhnrl 57:ec69651c8c21 1142
tnhnrl 57:ec69651c8c21 1143 /***************************** DEBUG MENU *****************************/
tnhnrl 57:ec69651c8c21 1144 if (_debug_menu_on) {
tnhnrl 17:7c16b5671d0e 1145
tnhnrl 57:ec69651c8c21 1146 if (user_input == 'D') {
tnhnrl 57:ec69651c8c21 1147 _keyboard_state = DIVE;
tnhnrl 57:ec69651c8c21 1148 }
tnhnrl 57:ec69651c8c21 1149 else if (user_input == 'N') {
tnhnrl 57:ec69651c8c21 1150 _keyboard_state = FIND_NEUTRAL;
tnhnrl 57:ec69651c8c21 1151 }
tnhnrl 57:ec69651c8c21 1152 else if (user_input == 'M') {
tnhnrl 57:ec69651c8c21 1153 //currently does not run if there is no file.
tnhnrl 57:ec69651c8c21 1154
tnhnrl 57:ec69651c8c21 1155 //need to add method to Sequence Controller that returns -1
tnhnrl 57:ec69651c8c21 1156 // or some check that insures you cannot run the dive sequence without a file
tnhnrl 57:ec69651c8c21 1157
tnhnrl 57:ec69651c8c21 1158 stateMachine().getDiveSequence(); //get first sequence on keyboard press
tnhnrl 57:ec69651c8c21 1159 _keyboard_state = currentStateStruct.state;
tnhnrl 57:ec69651c8c21 1160
tnhnrl 58:94b7fd55185e 1161 pc().printf("Starting Dive Sequence Controller! (state: %d)\r\n", _keyboard_state); //neutral sequence and dive cycles
tnhnrl 57:ec69651c8c21 1162 }
tnhnrl 57:ec69651c8c21 1163 else if (user_input == 'R') {
tnhnrl 57:ec69651c8c21 1164 _keyboard_state = RISE;
tnhnrl 57:ec69651c8c21 1165 }
tnhnrl 57:ec69651c8c21 1166 else if (user_input == 'J') {
tnhnrl 57:ec69651c8c21 1167 _keyboard_state = FLOAT_LEVEL;
tnhnrl 57:ec69651c8c21 1168 }
tnhnrl 57:ec69651c8c21 1169 else if (user_input == 'B') {
tnhnrl 57:ec69651c8c21 1170 _keyboard_state = FLOAT_BROADCAST;
tnhnrl 57:ec69651c8c21 1171 }
tnhnrl 57:ec69651c8c21 1172 else if (user_input == 'E') {
tnhnrl 57:ec69651c8c21 1173 _keyboard_state = EMERGENCY_CLIMB;
tnhnrl 57:ec69651c8c21 1174 }
tnhnrl 17:7c16b5671d0e 1175
tnhnrl 57:ec69651c8c21 1176 else if (user_input == 'T') {
tnhnrl 57:ec69651c8c21 1177 _keyboard_state = CHECK_TUNING;
tnhnrl 57:ec69651c8c21 1178 }
tnhnrl 17:7c16b5671d0e 1179
tnhnrl 58:94b7fd55185e 1180 else if (user_input == 'V') {
tnhnrl 58:94b7fd55185e 1181 _keyboard_state = POSITION_DIVE;
tnhnrl 58:94b7fd55185e 1182 }
tnhnrl 58:94b7fd55185e 1183
tnhnrl 57:ec69651c8c21 1184 // some debug tools below
tnhnrl 57:ec69651c8c21 1185 else if (user_input == 'P') {
tnhnrl 57:ec69651c8c21 1186 //Print current SD card log file
tnhnrl 57:ec69651c8c21 1187 //printCurrentSdLog();
tnhnrl 57:ec69651c8c21 1188 mbedLogger().printCurrentLogFile(); //print the current log file to the screen
tnhnrl 57:ec69651c8c21 1189 }
tnhnrl 57:ec69651c8c21 1190 else if (user_input == 'X') {
tnhnrl 57:ec69651c8c21 1191 mbedLogger().printMbedDirectory(); //print all log files to the screen
tnhnrl 57:ec69651c8c21 1192 }
tnhnrl 57:ec69651c8c21 1193 else if (user_input == 'O') {
tnhnrl 63:6cb0405fc6e6 1194 _keyboard_state = TRANSMIT_MBED_LOG; //Transmit data (work in progress)
tnhnrl 57:ec69651c8c21 1195 }
tnhnrl 57:ec69651c8c21 1196 else if (user_input == 'I') {
tnhnrl 57:ec69651c8c21 1197 mbedLogger().receiveMissionDataWithTicker(); //receive sequence.txt files
tnhnrl 57:ec69651c8c21 1198 }
tnhnrl 57:ec69651c8c21 1199 else if (user_input == '~') {
tnhnrl 58:94b7fd55185e 1200 pc().printf("ERASING MBED LOG FILE\r\n");
tnhnrl 57:ec69651c8c21 1201 mbedLogger().eraseFile();
tnhnrl 57:ec69651c8c21 1202 }
tnhnrl 57:ec69651c8c21 1203 else if (user_input == 'Z') {
tnhnrl 58:94b7fd55185e 1204 pc().printf("FSG FSM States: \r\n");
tnhnrl 57:ec69651c8c21 1205 string string_state;
tnhnrl 57:ec69651c8c21 1206
tnhnrl 57:ec69651c8c21 1207 for (int i = 0; i < _state_array_counter; i++) {
tnhnrl 57:ec69651c8c21 1208 if (_state_array[i] == SIT_IDLE)
tnhnrl 57:ec69651c8c21 1209 string_state = "SIT_IDLE <END>";
tnhnrl 57:ec69651c8c21 1210 else if (_state_array[i] == FIND_NEUTRAL)
tnhnrl 57:ec69651c8c21 1211 string_state = "FIND_NEUTRAL";
tnhnrl 57:ec69651c8c21 1212 else if (_state_array[i] == DIVE)
tnhnrl 57:ec69651c8c21 1213 string_state = "DIVE";
tnhnrl 57:ec69651c8c21 1214 else if (_state_array[i] == RISE)
tnhnrl 57:ec69651c8c21 1215 string_state = "RISE";
tnhnrl 57:ec69651c8c21 1216 else if (_state_array[i] == FLOAT_LEVEL)
tnhnrl 57:ec69651c8c21 1217 string_state = "FLOAT_LEVEL";
tnhnrl 57:ec69651c8c21 1218 else if (_state_array[i] == FLOAT_BROADCAST)
tnhnrl 57:ec69651c8c21 1219 string_state = "FLOAT_BROADCAST";
tnhnrl 57:ec69651c8c21 1220 else if (_state_array[i] == EMERGENCY_CLIMB)
tnhnrl 57:ec69651c8c21 1221 string_state = "EMERGENCY_CLIMB";
tnhnrl 57:ec69651c8c21 1222 else if (_state_array[i] == MULTI_DIVE)
tnhnrl 57:ec69651c8c21 1223 string_state = "MULTI_DIVE";
tnhnrl 57:ec69651c8c21 1224 else if (_state_array[i] == MULTI_RISE)
tnhnrl 57:ec69651c8c21 1225 string_state = "MULTI_RISE";
tnhnrl 57:ec69651c8c21 1226 else if (_state_array[i] == KEYBOARD)
tnhnrl 57:ec69651c8c21 1227 string_state = "KEYBOARD";
tnhnrl 58:94b7fd55185e 1228 pc().printf("State #%d: %d (%s)\r\n", i, _state_array[i], string_state.c_str());
tnhnrl 57:ec69651c8c21 1229 }
tnhnrl 57:ec69651c8c21 1230
tnhnrl 58:94b7fd55185e 1231 pc().printf("\r\nNeutral sub-FSM States: \r\n");
tnhnrl 57:ec69651c8c21 1232 string string_substate;
tnhnrl 57:ec69651c8c21 1233
tnhnrl 57:ec69651c8c21 1234 for (int i = 0; i < _substate_array_counter; i++) {
tnhnrl 57:ec69651c8c21 1235 if (_substate_array[i] == NEUTRAL_SINKING)
tnhnrl 57:ec69651c8c21 1236 string_substate = "NEUTRAL_SINKING";
tnhnrl 57:ec69651c8c21 1237 else if (_substate_array[i] == NEUTRAL_SLOWLY_RISE)
tnhnrl 57:ec69651c8c21 1238 string_substate = "NEUTRAL_SLOWLY_RISE";
tnhnrl 57:ec69651c8c21 1239 else if (_substate_array[i] == NEUTRAL_CHECK_PITCH)
tnhnrl 57:ec69651c8c21 1240 string_substate = "NEUTRAL_CHECK_PITCH";
tnhnrl 57:ec69651c8c21 1241 else if (_substate_array[i] == NEUTRAL_EXIT)
tnhnrl 57:ec69651c8c21 1242 string_substate = "NEUTRAL_EXIT <-- ";
tnhnrl 57:ec69651c8c21 1243 else if (_substate_array[i] == EMERGENCY_CLIMB)
tnhnrl 57:ec69651c8c21 1244 string_substate = " -- > EMERGENCY_CLIMB <-- ";
tnhnrl 58:94b7fd55185e 1245 pc().printf("Neutral Substate #%d: %d (%s)\r\n", i, _state_array[i], string_substate.c_str());
tnhnrl 57:ec69651c8c21 1246 }
tnhnrl 58:94b7fd55185e 1247 pc().printf("\r\n"); //make space between printouts
tnhnrl 57:ec69651c8c21 1248 }
tnhnrl 57:ec69651c8c21 1249 else if (user_input == '|') {
tnhnrl 57:ec69651c8c21 1250 pc().printf("taring depth sensor\r\n");
tnhnrl 57:ec69651c8c21 1251 pc().printf("Pre-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
tnhnrl 57:ec69651c8c21 1252 wait(0.1);
tnhnrl 57:ec69651c8c21 1253 depth().tare(); // tares to ambient (do on surface)
tnhnrl 57:ec69651c8c21 1254 pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
tnhnrl 57:ec69651c8c21 1255 }
tnhnrl 28:16c83a2fdefa 1256
tnhnrl 57:ec69651c8c21 1257 else if (user_input == '[' or user_input == '{') {
tnhnrl 57:ec69651c8c21 1258 _neutral_bce_pos_mm = depthLoop().getOutputOffset() - 1;
tnhnrl 57:ec69651c8c21 1259 depthLoop().setOutputOffset(_neutral_bce_pos_mm); // decrease the bce neutral setpoint
tnhnrl 57:ec69651c8c21 1260 pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 57:ec69651c8c21 1261 // save neutral depth value to config file
tnhnrl 57:ec69651c8c21 1262 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
tnhnrl 57:ec69651c8c21 1263 }
tnhnrl 57:ec69651c8c21 1264 else if (user_input == ']' or user_input == '}') {
tnhnrl 57:ec69651c8c21 1265 _neutral_bce_pos_mm = depthLoop().getOutputOffset() + 1;
tnhnrl 57:ec69651c8c21 1266 depthLoop().setOutputOffset(_neutral_bce_pos_mm); // increase the bce neutral setpoint
tnhnrl 57:ec69651c8c21 1267 pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 57:ec69651c8c21 1268 // save neutral depth value to config file
tnhnrl 57:ec69651c8c21 1269 configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
tnhnrl 57:ec69651c8c21 1270 }
tnhnrl 57:ec69651c8c21 1271 else if (user_input == '<' or user_input == ',') {
tnhnrl 57:ec69651c8c21 1272 _neutral_batt_pos_mm = pitchLoop().getOutputOffset() - 1;
tnhnrl 57:ec69651c8c21 1273 pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // decrease the batt neutral setpoint
tnhnrl 57:ec69651c8c21 1274 pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 57:ec69651c8c21 1275 // save neutral pitch value to config file
tnhnrl 57:ec69651c8c21 1276 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
tnhnrl 57:ec69651c8c21 1277 }
tnhnrl 57:ec69651c8c21 1278 else if (user_input == '>' or user_input == '.') {
tnhnrl 57:ec69651c8c21 1279 _neutral_batt_pos_mm = pitchLoop().getOutputOffset() + 1;
tnhnrl 57:ec69651c8c21 1280 pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // increase the batt neutral setpoint
tnhnrl 57:ec69651c8c21 1281 pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 57:ec69651c8c21 1282 // save neutral pitch value to config file
tnhnrl 57:ec69651c8c21 1283 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
tnhnrl 28:16c83a2fdefa 1284 }
tnhnrl 28:16c83a2fdefa 1285
tnhnrl 57:ec69651c8c21 1286 else if (user_input == '?') {
tnhnrl 57:ec69651c8c21 1287 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
tnhnrl 57:ec69651c8c21 1288 wait(0.5);
tnhnrl 57:ec69651c8c21 1289 mbed_reset();
tnhnrl 57:ec69651c8c21 1290 }
tnhnrl 57:ec69651c8c21 1291
tnhnrl 57:ec69651c8c21 1292 // change settings
tnhnrl 57:ec69651c8c21 1293 else if (user_input == 'Q' or user_input == 'q') {
tnhnrl 57:ec69651c8c21 1294 _pitch_command -= 0.5; //decrement the pitch setpoint
tnhnrl 57:ec69651c8c21 1295 pitchLoop().setCommand(_pitch_command);
tnhnrl 57:ec69651c8c21 1296 pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
tnhnrl 57:ec69651c8c21 1297 }
tnhnrl 57:ec69651c8c21 1298 else if (user_input == 'W' or user_input == 'w') {
tnhnrl 57:ec69651c8c21 1299 _pitch_command += 0.5; //increment the pitch setpoint
tnhnrl 57:ec69651c8c21 1300 pitchLoop().setCommand(_pitch_command);
tnhnrl 57:ec69651c8c21 1301 pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
tnhnrl 57:ec69651c8c21 1302 }
tnhnrl 57:ec69651c8c21 1303 else if (user_input == 'A' or user_input == 'a') {
tnhnrl 57:ec69651c8c21 1304 _depth_command -= 0.5; //decrement the depth setpoint
tnhnrl 57:ec69651c8c21 1305 depthLoop().setCommand(_depth_command);
tnhnrl 57:ec69651c8c21 1306 pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
tnhnrl 57:ec69651c8c21 1307 }
tnhnrl 57:ec69651c8c21 1308 else if (user_input == 'S' or user_input == 's') {
tnhnrl 57:ec69651c8c21 1309 _depth_command += 0.5; //increment the depth setpoint
tnhnrl 57:ec69651c8c21 1310 depthLoop().setCommand(_depth_command);
tnhnrl 57:ec69651c8c21 1311 pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
tnhnrl 57:ec69651c8c21 1312 }
tnhnrl 57:ec69651c8c21 1313 else if (user_input == '-') {
tnhnrl 57:ec69651c8c21 1314 _timeout -= 10.0; //decrement the timeout
tnhnrl 57:ec69651c8c21 1315 pc().printf(">>> timeout decreased: %d\r\n", _timeout);
tnhnrl 57:ec69651c8c21 1316 }
tnhnrl 57:ec69651c8c21 1317 else if (user_input == '=' or user_input == '+') {
tnhnrl 57:ec69651c8c21 1318 _timeout += 10.0; //increment the timeout
tnhnrl 57:ec69651c8c21 1319 pc().printf(">>> timeout increased: %d\r\n", _timeout);
tnhnrl 57:ec69651c8c21 1320 }
tnhnrl 28:16c83a2fdefa 1321
tnhnrl 57:ec69651c8c21 1322 else if (user_input == '5') {
tnhnrl 58:94b7fd55185e 1323 keyboard_menu_RUDDER_SERVO_settings();
tnhnrl 57:ec69651c8c21 1324 }
tnhnrl 57:ec69651c8c21 1325
tnhnrl 57:ec69651c8c21 1326 else if (user_input == '6') {
tnhnrl 58:94b7fd55185e 1327 keyboard_menu_HEADING_PID_settings();
tnhnrl 58:94b7fd55185e 1328 }
tnhnrl 57:ec69651c8c21 1329
tnhnrl 58:94b7fd55185e 1330 // go to tuning sub-menu
tnhnrl 57:ec69651c8c21 1331 else if (user_input == '7') {
tnhnrl 58:94b7fd55185e 1332 keyboard_menu_MANUAL_TUNING();
tnhnrl 57:ec69651c8c21 1333 }
tnhnrl 57:ec69651c8c21 1334
tnhnrl 57:ec69651c8c21 1335 else if (user_input == '8') {
tnhnrl 58:94b7fd55185e 1336 keyboard_menu_STREAM_STATUS(); //and channel readings for debugging
tnhnrl 57:ec69651c8c21 1337 }
tnhnrl 58:94b7fd55185e 1338
tnhnrl 58:94b7fd55185e 1339 // else if (user_input == '8') {
tnhnrl 58:94b7fd55185e 1340 // keyboard_menu_CHANNEL_READINGS();
tnhnrl 58:94b7fd55185e 1341 // }
tnhnrl 58:94b7fd55185e 1342
tnhnrl 57:ec69651c8c21 1343 else if (user_input == '9') {
tnhnrl 58:94b7fd55185e 1344 _heading_command -= 5.0; //decrement the rudder setpoint
tnhnrl 58:94b7fd55185e 1345 headingLoop().setCommand(_heading_command);
tnhnrl 58:94b7fd55185e 1346 pc().printf(">>> (-) new HEADING setpoint: %0.3f deg (-)\r\n", headingLoop().getCommand());
tnhnrl 58:94b7fd55185e 1347 }
tnhnrl 58:94b7fd55185e 1348 else if (user_input == '0') {
tnhnrl 58:94b7fd55185e 1349 _heading_command += 5.0; //increment the rudder setpoint
tnhnrl 58:94b7fd55185e 1350 headingLoop().setCommand(_heading_command);
tnhnrl 58:94b7fd55185e 1351 pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
tnhnrl 58:94b7fd55185e 1352 }
tnhnrl 58:94b7fd55185e 1353 else if (user_input == 'Y') {
tnhnrl 57:ec69651c8c21 1354 keyboard_menu_POSITION_READINGS();
tnhnrl 57:ec69651c8c21 1355 }
tnhnrl 57:ec69651c8c21 1356
tnhnrl 57:ec69651c8c21 1357 // go to sub-menus for the PID gains (this is blocking)
tnhnrl 57:ec69651c8c21 1358 else if (user_input == '1') {
tnhnrl 57:ec69651c8c21 1359 keyboard_menu_BCE_PID_settings();
tnhnrl 57:ec69651c8c21 1360 }
tnhnrl 57:ec69651c8c21 1361 else if (user_input == '2') {
tnhnrl 57:ec69651c8c21 1362 keyboard_menu_BATT_PID_settings();
tnhnrl 57:ec69651c8c21 1363 }
tnhnrl 57:ec69651c8c21 1364 else if (user_input == '3') {
tnhnrl 57:ec69651c8c21 1365 keyboard_menu_DEPTH_PID_settings();
tnhnrl 57:ec69651c8c21 1366 }
tnhnrl 57:ec69651c8c21 1367 else if (user_input == '4') {
tnhnrl 57:ec69651c8c21 1368 keyboard_menu_PITCH_PID_settings();
tnhnrl 28:16c83a2fdefa 1369 }
tnhnrl 57:ec69651c8c21 1370
tnhnrl 57:ec69651c8c21 1371 else if (user_input == 'C' or user_input == 'c') {
tnhnrl 57:ec69651c8c21 1372
tnhnrl 58:94b7fd55185e 1373 pc().printf("\r\n\nCURRENT STATUS AND PARAMETERS:\r\n");
tnhnrl 57:ec69651c8c21 1374
tnhnrl 57:ec69651c8c21 1375 // Testing out ADC
tnhnrl 57:ec69651c8c21 1376 float vref = 5.6;
tnhnrl 57:ec69651c8c21 1377 float vmeasured = 0;
tnhnrl 57:ec69651c8c21 1378 unsigned int raw = adc().readCh5();
tnhnrl 57:ec69651c8c21 1379 vmeasured = ((float)raw)/4095.0*vref;
tnhnrl 57:ec69651c8c21 1380 pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
tnhnrl 57:ec69651c8c21 1381 pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
tnhnrl 57:ec69651c8c21 1382 pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
tnhnrl 57:ec69651c8c21 1383 pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
tnhnrl 57:ec69651c8c21 1384 pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
tnhnrl 57:ec69651c8c21 1385 pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
tnhnrl 57:ec69651c8c21 1386 pc().printf("raw battery voltage: %d \r\n",adc().readCh6());
tnhnrl 57:ec69651c8c21 1387 pc().printf("raw board current: %d \r\n",adc().readCh7());
tnhnrl 57:ec69651c8c21 1388 pc().printf("raw BCE limit switch: %d \r\n",bce().getSwitch());
tnhnrl 57:ec69651c8c21 1389 pc().printf("raw BMM limit switch: %d \r\n",batt().getSwitch());
tnhnrl 57:ec69651c8c21 1390 pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
tnhnrl 57:ec69651c8c21 1391 // End of ADC Test
tnhnrl 57:ec69651c8c21 1392
tnhnrl 57:ec69651c8c21 1393 pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
tnhnrl 57:ec69651c8c21 1394 pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
tnhnrl 57:ec69651c8c21 1395 pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
tnhnrl 57:ec69651c8c21 1396 pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
tnhnrl 57:ec69651c8c21 1397 pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
tnhnrl 57:ec69651c8c21 1398 pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
tnhnrl 57:ec69651c8c21 1399 pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
tnhnrl 57:ec69651c8c21 1400 pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 57:ec69651c8c21 1401
tnhnrl 58:94b7fd55185e 1402 pc().printf("\r\nNeutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
tnhnrl 57:ec69651c8c21 1403 pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 57:ec69651c8c21 1404 pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 58:94b7fd55185e 1405 pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\r\n\n",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
tnhnrl 57:ec69651c8c21 1406
tnhnrl 58:94b7fd55185e 1407 pc().printf("\r\n");
tnhnrl 57:ec69651c8c21 1408 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 57:ec69651c8c21 1409 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 57:ec69651c8c21 1410 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 57:ec69651c8c21 1411 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 57:ec69651c8c21 1412 }
tnhnrl 57:ec69651c8c21 1413
tnhnrl 57:ec69651c8c21 1414 //POSITION DIVE COMMANDS
tnhnrl 57:ec69651c8c21 1415 else if (user_input == 'k') {
tnhnrl 58:94b7fd55185e 1416 _BCE_dive_offset -= 1.0;
tnhnrl 58:94b7fd55185e 1417 pc().printf("Decreased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
tnhnrl 57:ec69651c8c21 1418 }
tnhnrl 57:ec69651c8c21 1419 else if (user_input == 'l') {
tnhnrl 58:94b7fd55185e 1420 _BCE_dive_offset += 1.0;
tnhnrl 58:94b7fd55185e 1421 pc().printf("Increased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
tnhnrl 57:ec69651c8c21 1422 }
tnhnrl 57:ec69651c8c21 1423 else if (user_input == ';') {
tnhnrl 58:94b7fd55185e 1424 _BMM_dive_offset -= 1.0;
tnhnrl 58:94b7fd55185e 1425 pc().printf("Decreased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
tnhnrl 57:ec69651c8c21 1426 }
tnhnrl 57:ec69651c8c21 1427 else if (user_input == '\'') {
tnhnrl 58:94b7fd55185e 1428 _BMM_dive_offset += 1.0;
tnhnrl 58:94b7fd55185e 1429 pc().printf("Increased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
tnhnrl 57:ec69651c8c21 1430 }
tnhnrl 57:ec69651c8c21 1431 //POSITION DIVE COMMANDS
tnhnrl 57:ec69651c8c21 1432
tnhnrl 57:ec69651c8c21 1433 else if (user_input == '*') {
tnhnrl 58:94b7fd55185e 1434 pc().printf("SWITCHING TO SIMPLE MENU!\r\n");
tnhnrl 57:ec69651c8c21 1435 wait(2);
tnhnrl 57:ec69651c8c21 1436 _debug_menu_on = false;
tnhnrl 57:ec69651c8c21 1437 }
tnhnrl 57:ec69651c8c21 1438 } //end of debug menu
tnhnrl 57:ec69651c8c21 1439 /***************************** DEBUG MENU *****************************/
tnhnrl 57:ec69651c8c21 1440
tnhnrl 57:ec69651c8c21 1441 /***************************** SIMPLE MENU *****************************/
tnhnrl 57:ec69651c8c21 1442 else {
tnhnrl 58:94b7fd55185e 1443 if (user_input == 'V') {
tnhnrl 58:94b7fd55185e 1444 _keyboard_state = POSITION_DIVE;
tnhnrl 57:ec69651c8c21 1445 }
tnhnrl 57:ec69651c8c21 1446 else if (user_input == 'N') {
tnhnrl 57:ec69651c8c21 1447 _keyboard_state = FIND_NEUTRAL;
tnhnrl 57:ec69651c8c21 1448 }
tnhnrl 57:ec69651c8c21 1449 else if (user_input == 'J') {
tnhnrl 57:ec69651c8c21 1450 _keyboard_state = FLOAT_LEVEL;
tnhnrl 57:ec69651c8c21 1451 }
tnhnrl 57:ec69651c8c21 1452 else if (user_input == 'B') {
tnhnrl 57:ec69651c8c21 1453 _keyboard_state = FLOAT_BROADCAST;
tnhnrl 57:ec69651c8c21 1454 }
tnhnrl 57:ec69651c8c21 1455 else if (user_input == 'E') {
tnhnrl 57:ec69651c8c21 1456 _keyboard_state = EMERGENCY_CLIMB;
tnhnrl 57:ec69651c8c21 1457 }
tnhnrl 57:ec69651c8c21 1458
tnhnrl 57:ec69651c8c21 1459 // some debug tools below
tnhnrl 57:ec69651c8c21 1460 else if (user_input == 'P') {
tnhnrl 57:ec69651c8c21 1461 //Print current SD card log file
tnhnrl 57:ec69651c8c21 1462 //printCurrentSdLog();
tnhnrl 57:ec69651c8c21 1463 mbedLogger().printCurrentLogFile(); //print the current log file to the screen
tnhnrl 57:ec69651c8c21 1464 }
tnhnrl 57:ec69651c8c21 1465 else if (user_input == 'O') {
tnhnrl 63:6cb0405fc6e6 1466 _keyboard_state = TRANSMIT_MBED_LOG; //Transmit data (work in progress)
tnhnrl 57:ec69651c8c21 1467 }
tnhnrl 57:ec69651c8c21 1468 else if (user_input == '~') {
tnhnrl 58:94b7fd55185e 1469 pc().printf("ERASING MBED LOG FILE\r\n");
tnhnrl 57:ec69651c8c21 1470 mbedLogger().eraseFile();
tnhnrl 57:ec69651c8c21 1471 }
tnhnrl 57:ec69651c8c21 1472
tnhnrl 58:94b7fd55185e 1473 else if (user_input == 'C' or user_input == 'c') {
tnhnrl 58:94b7fd55185e 1474
tnhnrl 58:94b7fd55185e 1475 pc().printf("\r\n\nCURRENT STATUS AND PARAMETERS:\r\n");
tnhnrl 58:94b7fd55185e 1476
tnhnrl 58:94b7fd55185e 1477 // Testing out ADC
tnhnrl 58:94b7fd55185e 1478 float vref = 5.6;
tnhnrl 58:94b7fd55185e 1479 float vmeasured = 0;
tnhnrl 58:94b7fd55185e 1480 unsigned int raw = adc().readCh5();
tnhnrl 58:94b7fd55185e 1481 vmeasured = ((float)raw)/4095.0*vref;
tnhnrl 58:94b7fd55185e 1482 pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
tnhnrl 58:94b7fd55185e 1483 pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
tnhnrl 58:94b7fd55185e 1484 pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
tnhnrl 58:94b7fd55185e 1485 pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
tnhnrl 58:94b7fd55185e 1486 pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
tnhnrl 58:94b7fd55185e 1487 pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
tnhnrl 58:94b7fd55185e 1488 pc().printf("raw battery voltage: %d \r\n",adc().readCh6());
tnhnrl 58:94b7fd55185e 1489 pc().printf("raw board current: %d \r\n",adc().readCh7());
tnhnrl 58:94b7fd55185e 1490 pc().printf("raw BCE limit switch: %d \r\n",bce().getSwitch());
tnhnrl 58:94b7fd55185e 1491 pc().printf("raw BMM limit switch: %d \r\n",batt().getSwitch());
tnhnrl 58:94b7fd55185e 1492 pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
tnhnrl 58:94b7fd55185e 1493 // End of ADC Test
tnhnrl 58:94b7fd55185e 1494
tnhnrl 58:94b7fd55185e 1495 pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
tnhnrl 58:94b7fd55185e 1496 pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
tnhnrl 58:94b7fd55185e 1497 pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
tnhnrl 58:94b7fd55185e 1498 pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
tnhnrl 58:94b7fd55185e 1499 pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
tnhnrl 58:94b7fd55185e 1500 pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
tnhnrl 58:94b7fd55185e 1501 pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
tnhnrl 58:94b7fd55185e 1502 pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 58:94b7fd55185e 1503
tnhnrl 58:94b7fd55185e 1504 pc().printf("\r\nNeutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
tnhnrl 58:94b7fd55185e 1505 pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 58:94b7fd55185e 1506 pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 58:94b7fd55185e 1507 pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\r\n\n",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
tnhnrl 58:94b7fd55185e 1508
tnhnrl 58:94b7fd55185e 1509 pc().printf("\r\n");
tnhnrl 58:94b7fd55185e 1510 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 58:94b7fd55185e 1511 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 58:94b7fd55185e 1512 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 58:94b7fd55185e 1513 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 58:94b7fd55185e 1514 }
tnhnrl 58:94b7fd55185e 1515
tnhnrl 58:94b7fd55185e 1516 else if (user_input == '-') {
tnhnrl 58:94b7fd55185e 1517 _timeout -= 10.0; //decrement the timeout
tnhnrl 58:94b7fd55185e 1518 pc().printf(">>> timeout decreased: %d\r\n", _timeout);
tnhnrl 58:94b7fd55185e 1519 }
tnhnrl 58:94b7fd55185e 1520 else if (user_input == '=' or user_input == '+') {
tnhnrl 58:94b7fd55185e 1521 _timeout += 10.0; //increment the timeout
tnhnrl 58:94b7fd55185e 1522 pc().printf(">>> timeout increased: %d\r\n", _timeout);
tnhnrl 58:94b7fd55185e 1523 }
tnhnrl 58:94b7fd55185e 1524
tnhnrl 57:ec69651c8c21 1525 else if (user_input == '?') {
tnhnrl 57:ec69651c8c21 1526 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
tnhnrl 57:ec69651c8c21 1527 wait(0.5);
tnhnrl 57:ec69651c8c21 1528 mbed_reset();
tnhnrl 57:ec69651c8c21 1529 }
tnhnrl 57:ec69651c8c21 1530
tnhnrl 57:ec69651c8c21 1531 //POSITION DIVE COMMANDS
tnhnrl 57:ec69651c8c21 1532 else if (user_input == 'k') {
tnhnrl 58:94b7fd55185e 1533 _BCE_dive_offset -= 1.0;
tnhnrl 58:94b7fd55185e 1534 pc().printf("Decreased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
tnhnrl 57:ec69651c8c21 1535 }
tnhnrl 57:ec69651c8c21 1536 else if (user_input == 'l') {
tnhnrl 58:94b7fd55185e 1537 _BCE_dive_offset += 1.0;
tnhnrl 58:94b7fd55185e 1538 pc().printf("Increased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
tnhnrl 57:ec69651c8c21 1539 }
tnhnrl 57:ec69651c8c21 1540 else if (user_input == ';') {
tnhnrl 58:94b7fd55185e 1541 _BMM_dive_offset -= 1.0;
tnhnrl 58:94b7fd55185e 1542 pc().printf("Decreased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
tnhnrl 57:ec69651c8c21 1543 }
tnhnrl 57:ec69651c8c21 1544 else if (user_input == '\'') {
tnhnrl 58:94b7fd55185e 1545 _BMM_dive_offset += 1.0;
tnhnrl 58:94b7fd55185e 1546 pc().printf("Increased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
tnhnrl 57:ec69651c8c21 1547 }
tnhnrl 57:ec69651c8c21 1548 //POSITION DIVE COMMANDS
tnhnrl 58:94b7fd55185e 1549
tnhnrl 58:94b7fd55185e 1550 else if (user_input == '9') {
tnhnrl 58:94b7fd55185e 1551 _heading_command -= 5.0; //decrement the rudder setpoint
tnhnrl 58:94b7fd55185e 1552 headingLoop().setCommand(_heading_command);
tnhnrl 58:94b7fd55185e 1553 pc().printf(">>> (-) new HEADING setpoint: %0.3f deg (-)\r\n", headingLoop().getCommand());
tnhnrl 58:94b7fd55185e 1554 }
tnhnrl 58:94b7fd55185e 1555 else if (user_input == '0') {
tnhnrl 58:94b7fd55185e 1556 _heading_command += 5.0; //increment the rudder setpoint
tnhnrl 58:94b7fd55185e 1557 headingLoop().setCommand(_heading_command);
tnhnrl 58:94b7fd55185e 1558 pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
tnhnrl 58:94b7fd55185e 1559 }
tnhnrl 58:94b7fd55185e 1560
tnhnrl 58:94b7fd55185e 1561 else if (user_input == '8') {
tnhnrl 58:94b7fd55185e 1562 keyboard_menu_STREAM_STATUS();
tnhnrl 58:94b7fd55185e 1563 }
tnhnrl 57:ec69651c8c21 1564
tnhnrl 57:ec69651c8c21 1565 else if (user_input == '*') {
tnhnrl 58:94b7fd55185e 1566 pc().printf("SWITCHING TO DEBUG MENU!\r\n");
tnhnrl 57:ec69651c8c21 1567 _debug_menu_on = true;
tnhnrl 57:ec69651c8c21 1568 wait(2);
tnhnrl 57:ec69651c8c21 1569 }
tnhnrl 16:3363b9f14913 1570 }
tnhnrl 57:ec69651c8c21 1571 /***************************** SIMPLE MENU *****************************/
tnhnrl 17:7c16b5671d0e 1572
tnhnrl 17:7c16b5671d0e 1573 //when you read the keyboard successfully, change the state
tnhnrl 28:16c83a2fdefa 1574 _state = _keyboard_state; //set state at the end of this function
tnhnrl 58:94b7fd55185e 1575 //pc().printf("\r\n\n ********* KEYBOARD STATE: %d *********\r\n\n", _state);
tnhnrl 16:3363b9f14913 1576 }
tnhnrl 16:3363b9f14913 1577 }
tnhnrl 52:f207567d3ea4 1578
tnhnrl 52:f207567d3ea4 1579 void StateMachine::keyboard_menu_STREAM_STATUS() {
tnhnrl 52:f207567d3ea4 1580 char STATUS_key;
tnhnrl 52:f207567d3ea4 1581
tnhnrl 52:f207567d3ea4 1582 // show the menu
tnhnrl 58:94b7fd55185e 1583 pc().printf("\r\n8: STATUS DEBUG MENU (EXIT WITH 'X' !)\r\n");
tnhnrl 52:f207567d3ea4 1584
tnhnrl 52:f207567d3ea4 1585 while (1) {
tnhnrl 52:f207567d3ea4 1586 if (pc().readable()) {
tnhnrl 52:f207567d3ea4 1587 STATUS_key = pc().getc(); //get each keystroke
tnhnrl 52:f207567d3ea4 1588 }
tnhnrl 52:f207567d3ea4 1589
tnhnrl 52:f207567d3ea4 1590 else {
tnhnrl 52:f207567d3ea4 1591
tnhnrl 52:f207567d3ea4 1592 wait(1);
tnhnrl 54:d4990fb68404 1593
tnhnrl 58:94b7fd55185e 1594
tnhnrl 58:94b7fd55185e 1595 pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f [0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)]\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm(),adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7());
tnhnrl 58:94b7fd55185e 1596 //pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f (unfiltered: %0.2f) [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depth().getRawPSI(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm());
tnhnrl 58:94b7fd55185e 1597 //pc().printf("\n0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7());
tnhnrl 58:94b7fd55185e 1598
tnhnrl 54:d4990fb68404 1599
tnhnrl 56:48a8a5a65b82 1600 //pc().printf("(set) BCE POS: %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS: %0.2f )(channel reading: %d) (volt: %0.2f) << %0.2f >> (Pressure: %0.2f (raw: %0.2f) PITCH: %0.2f HEADING: %0.2f)\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(), depthLoop().getPosition(),depth().readADCCounts(),depth().readVoltage(),depth().getPsi(),depth().getRawPSI(),depth().getRawPSI(),imu().getPitch(),imu().getHeading());
tnhnrl 52:f207567d3ea4 1601
tnhnrl 54:d4990fb68404 1602 //pc().printf("Neutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
tnhnrl 54:d4990fb68404 1603 // pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
tnhnrl 54:d4990fb68404 1604 // pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
tnhnrl 54:d4990fb68404 1605 // pc().printf("heading (rudder): %3.1f deg\r\n",rudderLoop().getPosition()); //for heading
tnhnrl 52:f207567d3ea4 1606
tnhnrl 54:d4990fb68404 1607 // pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
tnhnrl 54:d4990fb68404 1608 // pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
tnhnrl 54:d4990fb68404 1609 //
tnhnrl 54:d4990fb68404 1610 // pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
tnhnrl 54:d4990fb68404 1611 // pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
tnhnrl 52:f207567d3ea4 1612
tnhnrl 54:d4990fb68404 1613 // pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
tnhnrl 54:d4990fb68404 1614 // pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 54:d4990fb68404 1615 //
tnhnrl 54:d4990fb68404 1616 // pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 54:d4990fb68404 1617 // pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 52:f207567d3ea4 1618
tnhnrl 52:f207567d3ea4 1619 continue; // didn't get a user input, so keep waiting for it
tnhnrl 52:f207567d3ea4 1620 }
tnhnrl 52:f207567d3ea4 1621
tnhnrl 52:f207567d3ea4 1622 // process the keys
tnhnrl 58:94b7fd55185e 1623 if (STATUS_key == 'X') {
tnhnrl 58:94b7fd55185e 1624 pc().printf("\r\nX: EXITING STATUS DEBUG MENU\r\n");
tnhnrl 52:f207567d3ea4 1625 break; //exit the while loop
tnhnrl 52:f207567d3ea4 1626 }
tnhnrl 52:f207567d3ea4 1627
tnhnrl 52:f207567d3ea4 1628 else {
tnhnrl 58:94b7fd55185e 1629 pc().printf("\r\nThis key (%c) does nothing here. ", STATUS_key);
tnhnrl 52:f207567d3ea4 1630 }
tnhnrl 52:f207567d3ea4 1631 }
tnhnrl 52:f207567d3ea4 1632 }
tnhnrl 52:f207567d3ea4 1633
tnhnrl 52:f207567d3ea4 1634 void StateMachine::keyboard_menu_RUDDER_SERVO_settings() {
tnhnrl 52:f207567d3ea4 1635 //load current parameters from the rudder
tnhnrl 52:f207567d3ea4 1636 float rudder_min_pwm = rudder().getMinPWM();
tnhnrl 52:f207567d3ea4 1637 float rudder_max_pwm = rudder().getMaxPWM();
tnhnrl 52:f207567d3ea4 1638 float rudder_ctr_pwm = rudder().getCenterPWM();
tnhnrl 52:f207567d3ea4 1639 float rudder_min_deg = rudder().getMinDeg();
tnhnrl 52:f207567d3ea4 1640 float rudder_max_deg = rudder().getMaxDeg();
tnhnrl 52:f207567d3ea4 1641
tnhnrl 52:f207567d3ea4 1642 char RUDDER_PID_key;
tnhnrl 52:f207567d3ea4 1643
tnhnrl 52:f207567d3ea4 1644 // print the menu
tnhnrl 58:94b7fd55185e 1645 pc().printf("\r\nRUDDER (servo driver) settings (MENU)");
tnhnrl 58:94b7fd55185e 1646 pc().printf("\r\n(Adjust min/max/center PWM settings with the following keys: N and M and C");
tnhnrl 58:94b7fd55185e 1647 pc().printf("\r\n(Adjust DEGREE limit settings with the following keys: min = K, max = L");
tnhnrl 58:94b7fd55185e 1648 pc().printf("\r\n(Hit shift + X to exit w/o saving. Hit shift + S to save.\r\n");
tnhnrl 52:f207567d3ea4 1649 pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
tnhnrl 52:f207567d3ea4 1650
tnhnrl 52:f207567d3ea4 1651 // handle the key presses
tnhnrl 52:f207567d3ea4 1652 while(1) {
tnhnrl 52:f207567d3ea4 1653 // get the user's keystroke from either of the two inputs
tnhnrl 52:f207567d3ea4 1654 if (pc().readable()) {
tnhnrl 52:f207567d3ea4 1655 RUDDER_PID_key = pc().getc();
tnhnrl 52:f207567d3ea4 1656 }
tnhnrl 52:f207567d3ea4 1657 else {
tnhnrl 52:f207567d3ea4 1658 wait(0.5);
tnhnrl 52:f207567d3ea4 1659 pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
tnhnrl 52:f207567d3ea4 1660 continue; // didn't get a user input, so keep waiting for it
tnhnrl 52:f207567d3ea4 1661 }
tnhnrl 52:f207567d3ea4 1662
tnhnrl 52:f207567d3ea4 1663 // handle the user's key input
tnhnrl 52:f207567d3ea4 1664 if (RUDDER_PID_key == 'S') { // user wants to save the modified values
tnhnrl 52:f207567d3ea4 1665 // set global values
tnhnrl 52:f207567d3ea4 1666 rudder().setMinPWM(rudder_min_pwm);
tnhnrl 52:f207567d3ea4 1667 rudder().setMaxPWM(rudder_max_pwm);
tnhnrl 52:f207567d3ea4 1668 rudder().setCenterPWM(rudder_ctr_pwm);
tnhnrl 52:f207567d3ea4 1669 rudder().setMinDeg(rudder_min_deg);
tnhnrl 52:f207567d3ea4 1670 rudder().setMaxDeg(rudder_max_deg);
tnhnrl 52:f207567d3ea4 1671
tnhnrl 52:f207567d3ea4 1672 // save rudder servo driver values for inner loop
tnhnrl 52:f207567d3ea4 1673 configFileIO().saveRudderData(rudder_min_deg, rudder_max_deg, rudder_ctr_pwm, rudder_min_pwm, rudder_max_pwm);
tnhnrl 52:f207567d3ea4 1674 pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
tnhnrl 52:f207567d3ea4 1675 }
tnhnrl 52:f207567d3ea4 1676 else if (RUDDER_PID_key == 'X') {
tnhnrl 52:f207567d3ea4 1677 break; //exit the while loop
tnhnrl 52:f207567d3ea4 1678 }
tnhnrl 52:f207567d3ea4 1679 // MIN PWM
tnhnrl 52:f207567d3ea4 1680 else if (RUDDER_PID_key == 'N') {
tnhnrl 58:94b7fd55185e 1681 pc().printf(">> Type in rudder_min_pwm with keyboard.\r\n");
tnhnrl 52:f207567d3ea4 1682 rudder_min_pwm = getFloatUserInput();
tnhnrl 52:f207567d3ea4 1683 }
tnhnrl 52:f207567d3ea4 1684 // MAX PWM
tnhnrl 52:f207567d3ea4 1685 else if (RUDDER_PID_key == 'M') {
tnhnrl 58:94b7fd55185e 1686 pc().printf(">> Type in rudder_max_pwm with keyboard.\r\n");
tnhnrl 52:f207567d3ea4 1687 rudder_max_pwm = getFloatUserInput();
tnhnrl 52:f207567d3ea4 1688 }
tnhnrl 52:f207567d3ea4 1689 // CENTER PWM
tnhnrl 52:f207567d3ea4 1690 else if (RUDDER_PID_key == 'C') {
tnhnrl 58:94b7fd55185e 1691 pc().printf(">> Type in rudder_ctr_pwm with keyboard.\r\n");
tnhnrl 52:f207567d3ea4 1692 rudder_ctr_pwm = getFloatUserInput();
tnhnrl 52:f207567d3ea4 1693 }
tnhnrl 52:f207567d3ea4 1694 // MIN DEG
tnhnrl 52:f207567d3ea4 1695 else if (RUDDER_PID_key == 'K') {
tnhnrl 58:94b7fd55185e 1696 pc().printf(">> Type in rudder_min_deg with keyboard.\r\n");
tnhnrl 52:f207567d3ea4 1697 rudder_min_deg = getFloatUserInput();
tnhnrl 52:f207567d3ea4 1698 }
tnhnrl 52:f207567d3ea4 1699 // MAX DEG
tnhnrl 52:f207567d3ea4 1700 else if (RUDDER_PID_key == 'L') {
tnhnrl 58:94b7fd55185e 1701 pc().printf(">> Type in rudder_max_deg with keyboard.\r\n");
tnhnrl 52:f207567d3ea4 1702 rudder_max_deg = getFloatUserInput();
tnhnrl 52:f207567d3ea4 1703 }
tnhnrl 52:f207567d3ea4 1704
tnhnrl 52:f207567d3ea4 1705 else {
tnhnrl 52:f207567d3ea4 1706 pc().printf("RUDDER SETUP: [%c] This key does nothing here. \r", RUDDER_PID_key);
tnhnrl 52:f207567d3ea4 1707 }
tnhnrl 52:f207567d3ea4 1708 }
tnhnrl 52:f207567d3ea4 1709 }
tnhnrl 54:d4990fb68404 1710
tnhnrl 58:94b7fd55185e 1711 void StateMachine::keyboard_menu_HEADING_PID_settings() {
tnhnrl 58:94b7fd55185e 1712 char HEADING_PID_key;
tnhnrl 58:94b7fd55185e 1713
tnhnrl 58:94b7fd55185e 1714 float heading_KP = headingLoop().getControllerP();
tnhnrl 58:94b7fd55185e 1715 float heading_KI = headingLoop().getControllerI();
tnhnrl 58:94b7fd55185e 1716 float heading_KD = headingLoop().getControllerD();
tnhnrl 58:94b7fd55185e 1717 float heading_offset_deg = headingLoop().getOutputOffset();
tnhnrl 58:94b7fd55185e 1718
tnhnrl 58:94b7fd55185e 1719 // print the menu
tnhnrl 58:94b7fd55185e 1720 pc().printf("\n\rHEADING (rudder outer loop) PID gain settings (MENU)");
tnhnrl 58:94b7fd55185e 1721 pc().printf("\n\r Adjust PID settings with the following keys: P and I and D");
tnhnrl 58:94b7fd55185e 1722 pc().printf("\n\r Adjust zero offset with O (oh)");
tnhnrl 58:94b7fd55185e 1723 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
tnhnrl 63:6cb0405fc6e6 1724 pc().printf("HEADING P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
tnhnrl 58:94b7fd55185e 1725
tnhnrl 58:94b7fd55185e 1726 // handle the key presses
tnhnrl 58:94b7fd55185e 1727 while(1) {
tnhnrl 58:94b7fd55185e 1728 // get the user's keystroke from either of the two inputs
tnhnrl 58:94b7fd55185e 1729 if (pc().readable()) {
tnhnrl 58:94b7fd55185e 1730 HEADING_PID_key = pc().getc();
tnhnrl 58:94b7fd55185e 1731 }
tnhnrl 58:94b7fd55185e 1732 else {
tnhnrl 58:94b7fd55185e 1733 continue; // didn't get a user input, so keep waiting for it
tnhnrl 58:94b7fd55185e 1734 }
tnhnrl 58:94b7fd55185e 1735
tnhnrl 58:94b7fd55185e 1736 // handle the user's key input
tnhnrl 58:94b7fd55185e 1737 if (HEADING_PID_key == 'S') { // user wants to save the modified values
tnhnrl 58:94b7fd55185e 1738 // set global values
tnhnrl 58:94b7fd55185e 1739 headingLoop().setControllerP(heading_KP);
tnhnrl 58:94b7fd55185e 1740 headingLoop().setControllerI(heading_KI);
tnhnrl 58:94b7fd55185e 1741 headingLoop().setControllerD(heading_KD);
tnhnrl 58:94b7fd55185e 1742 headingLoop().setOutputOffset(heading_offset_deg);
tnhnrl 58:94b7fd55185e 1743
tnhnrl 58:94b7fd55185e 1744 // save pitch PID values for outer loop (must save neutral position also)
tnhnrl 58:94b7fd55185e 1745 configFileIO().saveHeadingData(heading_KP, heading_KI, heading_KD, heading_offset_deg); //_neutral_heading_pos_deg);
tnhnrl 63:6cb0405fc6e6 1746 pc().printf("HEADING P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
tnhnrl 58:94b7fd55185e 1747 }
tnhnrl 58:94b7fd55185e 1748 else if (HEADING_PID_key == 'X') {
tnhnrl 58:94b7fd55185e 1749 break; //exit the while loop
tnhnrl 58:94b7fd55185e 1750 }
tnhnrl 58:94b7fd55185e 1751
tnhnrl 58:94b7fd55185e 1752 else if (HEADING_PID_key == 'P') {
tnhnrl 58:94b7fd55185e 1753 heading_KP = getFloatUserInput();;
tnhnrl 58:94b7fd55185e 1754 }
tnhnrl 58:94b7fd55185e 1755 else if (HEADING_PID_key == 'I') {
tnhnrl 58:94b7fd55185e 1756 heading_KI = getFloatUserInput();
tnhnrl 58:94b7fd55185e 1757 }
tnhnrl 58:94b7fd55185e 1758 else if (HEADING_PID_key == 'D') {
tnhnrl 58:94b7fd55185e 1759 heading_KD = getFloatUserInput();
tnhnrl 58:94b7fd55185e 1760 }
tnhnrl 58:94b7fd55185e 1761 else if (HEADING_PID_key == 'O') {
tnhnrl 58:94b7fd55185e 1762 heading_offset_deg = getFloatUserInput();
tnhnrl 58:94b7fd55185e 1763 }
tnhnrl 58:94b7fd55185e 1764
tnhnrl 58:94b7fd55185e 1765 else {
tnhnrl 58:94b7fd55185e 1766 pc().printf("HEADING SETUP: [%c] This key does nothing here. \r", HEADING_PID_key);
tnhnrl 58:94b7fd55185e 1767 }
tnhnrl 58:94b7fd55185e 1768 }
tnhnrl 58:94b7fd55185e 1769 }
tnhnrl 58:94b7fd55185e 1770
tnhnrl 54:d4990fb68404 1771 void StateMachine::keyboard_menu_COUNTS_STATUS() {
tnhnrl 54:d4990fb68404 1772
tnhnrl 54:d4990fb68404 1773 }
tnhnrl 56:48a8a5a65b82 1774
tnhnrl 49:47ffa4feb6db 1775 void StateMachine::keyboard_menu_MANUAL_TUNING() {
tnhnrl 49:47ffa4feb6db 1776 char TUNING_key;
tnhnrl 49:47ffa4feb6db 1777
tnhnrl 49:47ffa4feb6db 1778 // show the menu
tnhnrl 58:94b7fd55185e 1779 pc().printf("\r\n7: MANUAL TUNING MENU (EXIT WITH 'X' !) (Pause and Unpause rudder ticker with P and U\n");
tnhnrl 58:94b7fd55185e 1780 pc().printf("\r\n(Adjust BCE and BATT positions in real-time. Timeout NOT running! (decrease/increase BCE with A/S, BATT with Q/W, RUDDER with E/R)\r\n");
tnhnrl 58:94b7fd55185e 1781 pc().printf("\r\nMANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(), imu().getHeading());
tnhnrl 56:48a8a5a65b82 1782
tnhnrl 56:48a8a5a65b82 1783 //made these into internal parameters
tnhnrl 56:48a8a5a65b82 1784 float _tuning_bce_pos_mm = 300.0; //safe starting position
tnhnrl 56:48a8a5a65b82 1785 float _tuning_batt_pos_mm = 60.0; //safe starting position
tnhnrl 56:48a8a5a65b82 1786 float _tuning_rudder_pos_deg = 0.0; //safe starting position
tnhnrl 56:48a8a5a65b82 1787
tnhnrl 56:48a8a5a65b82 1788 //immediately start at those positions
tnhnrl 56:48a8a5a65b82 1789 bce().setPosition_mm(_tuning_bce_pos_mm);
tnhnrl 56:48a8a5a65b82 1790 batt().setPosition_mm(_tuning_batt_pos_mm);
tnhnrl 56:48a8a5a65b82 1791 rudder().setPosition_deg(_tuning_rudder_pos_deg);
tnhnrl 49:47ffa4feb6db 1792
tnhnrl 49:47ffa4feb6db 1793 // what needs to be started?
tnhnrl 49:47ffa4feb6db 1794 bce().unpause(); //this is now active
tnhnrl 49:47ffa4feb6db 1795 batt().unpause(); //this is now active
tnhnrl 56:48a8a5a65b82 1796 rudder().unpause();
tnhnrl 49:47ffa4feb6db 1797
tnhnrl 56:48a8a5a65b82 1798 while (1) {
tnhnrl 49:47ffa4feb6db 1799 if (pc().readable()) {
tnhnrl 49:47ffa4feb6db 1800 TUNING_key = pc().getc(); //get each keystroke
tnhnrl 49:47ffa4feb6db 1801 }
tnhnrl 49:47ffa4feb6db 1802
tnhnrl 58:94b7fd55185e 1803 else {
tnhnrl 58:94b7fd55185e 1804 pc().printf("MANUAL_TUNING: (current stats) BCE pos: %3.1f mm (cmd: %3.1f mm), BATT pos: %3.1f mm (cmd: %3.1f mm) SERVO: %0.1f deg, %f pwm\r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm());
tnhnrl 49:47ffa4feb6db 1805 continue; // didn't get a user input, so keep waiting for it
tnhnrl 49:47ffa4feb6db 1806 }
tnhnrl 49:47ffa4feb6db 1807
tnhnrl 49:47ffa4feb6db 1808 // process the keys
tnhnrl 49:47ffa4feb6db 1809 if (TUNING_key == 'X') {
tnhnrl 49:47ffa4feb6db 1810 // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
tnhnrl 49:47ffa4feb6db 1811 bce().pause();
tnhnrl 49:47ffa4feb6db 1812 batt().pause();
tnhnrl 56:48a8a5a65b82 1813 rudder().pause();
tnhnrl 56:48a8a5a65b82 1814
tnhnrl 56:48a8a5a65b82 1815 //right now the rudder is always active................................................hmm
tnhnrl 56:48a8a5a65b82 1816 //deactivate the pin? new/delete?
tnhnrl 49:47ffa4feb6db 1817
tnhnrl 49:47ffa4feb6db 1818 break; //exit the while loop
tnhnrl 49:47ffa4feb6db 1819 }
tnhnrl 56:48a8a5a65b82 1820
tnhnrl 56:48a8a5a65b82 1821 //Buoyancy Engine
tnhnrl 52:f207567d3ea4 1822 else if (TUNING_key == 'A') {
tnhnrl 56:48a8a5a65b82 1823 _tuning_bce_pos_mm = _tuning_bce_pos_mm - 1.0;
tnhnrl 56:48a8a5a65b82 1824 bce().setPosition_mm(_tuning_bce_pos_mm); //this variable is loaded from the file at initialization
tnhnrl 58:94b7fd55185e 1825 pc().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
tnhnrl 52:f207567d3ea4 1826 }
tnhnrl 52:f207567d3ea4 1827
tnhnrl 49:47ffa4feb6db 1828 else if (TUNING_key == 'S') {
tnhnrl 56:48a8a5a65b82 1829 _tuning_bce_pos_mm = _tuning_bce_pos_mm + 1.0;
tnhnrl 56:48a8a5a65b82 1830 bce().setPosition_mm(_tuning_bce_pos_mm); //this variable is loaded from the file at initialization
tnhnrl 58:94b7fd55185e 1831 pc().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
tnhnrl 49:47ffa4feb6db 1832 }
tnhnrl 49:47ffa4feb6db 1833
tnhnrl 56:48a8a5a65b82 1834 //BATTERY
tnhnrl 49:47ffa4feb6db 1835 else if (TUNING_key == 'Q') {
tnhnrl 56:48a8a5a65b82 1836 _tuning_batt_pos_mm = _tuning_batt_pos_mm - 1.0;
tnhnrl 56:48a8a5a65b82 1837 batt().setPosition_mm(_tuning_batt_pos_mm); //this variable is loaded from the file at initialization
tnhnrl 58:94b7fd55185e 1838 pc().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
tnhnrl 49:47ffa4feb6db 1839 }
tnhnrl 49:47ffa4feb6db 1840
tnhnrl 49:47ffa4feb6db 1841 else if (TUNING_key == 'W') {
tnhnrl 56:48a8a5a65b82 1842 _tuning_batt_pos_mm = _tuning_batt_pos_mm + 1.0;
tnhnrl 56:48a8a5a65b82 1843 batt().setPosition_mm(_tuning_batt_pos_mm); //this variable is loaded from the file at initialization
tnhnrl 58:94b7fd55185e 1844 pc().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
tnhnrl 56:48a8a5a65b82 1845 }
tnhnrl 56:48a8a5a65b82 1846
tnhnrl 56:48a8a5a65b82 1847 else if (TUNING_key == 'c' or TUNING_key == 'C') {
tnhnrl 56:48a8a5a65b82 1848 pc().printf("MANUAL_TUNING: (current stats) BCE pos: %3.1f mm (cmd: %3.1f mm), BATT pos: %3.1f mm (cmd: %3.1f mm) SERVO: %0.1f deg, %f pwm\r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm());
tnhnrl 56:48a8a5a65b82 1849 }
tnhnrl 56:48a8a5a65b82 1850
tnhnrl 56:48a8a5a65b82 1851 //RUDER
tnhnrl 56:48a8a5a65b82 1852 else if (TUNING_key == 'E') {
tnhnrl 56:48a8a5a65b82 1853 _tuning_rudder_pos_deg = _tuning_rudder_pos_deg - 0.5;
tnhnrl 56:48a8a5a65b82 1854 rudder().setPosition_deg(_tuning_rudder_pos_deg);
tnhnrl 58:94b7fd55185e 1855 pc().printf("MANUAL_TUNING: RUDDER CHANGE %0.1f deg [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r\n", _tuning_rudder_pos_deg, rudder().getPosition_pwm(), rudder().getPosition_deg(), headingLoop().getPosition(), imu().getHeading());
tnhnrl 56:48a8a5a65b82 1856 }
tnhnrl 56:48a8a5a65b82 1857
tnhnrl 56:48a8a5a65b82 1858 else if (TUNING_key == 'R') {
tnhnrl 56:48a8a5a65b82 1859 _tuning_rudder_pos_deg = _tuning_rudder_pos_deg + 0.5;
tnhnrl 56:48a8a5a65b82 1860 rudder().setPosition_deg(_tuning_rudder_pos_deg);
tnhnrl 58:94b7fd55185e 1861 pc().printf("MANUAL_TUNING: RUDDER CHANGE %0.1f deg [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r\n", _tuning_rudder_pos_deg, rudder().getPosition_pwm(), rudder().getPosition_deg(), headingLoop().getPosition(), imu().getHeading());
tnhnrl 56:48a8a5a65b82 1862 }
tnhnrl 56:48a8a5a65b82 1863
tnhnrl 56:48a8a5a65b82 1864 else if (TUNING_key == 'P') {
tnhnrl 56:48a8a5a65b82 1865 rudder().pause();
tnhnrl 56:48a8a5a65b82 1866 }
tnhnrl 56:48a8a5a65b82 1867
tnhnrl 56:48a8a5a65b82 1868 else if (TUNING_key == 'U') {
tnhnrl 56:48a8a5a65b82 1869 rudder().unpause();
tnhnrl 49:47ffa4feb6db 1870 }
tnhnrl 49:47ffa4feb6db 1871
tnhnrl 49:47ffa4feb6db 1872 else {
tnhnrl 58:94b7fd55185e 1873 pc().printf("\r\nMANUAL_TUNING: [%c] This key does nothing here. \r", TUNING_key);
tnhnrl 56:48a8a5a65b82 1874 }
tnhnrl 49:47ffa4feb6db 1875 }
tnhnrl 49:47ffa4feb6db 1876 }
tnhnrl 52:f207567d3ea4 1877
tnhnrl 52:f207567d3ea4 1878 void StateMachine::keyboard_menu_CHANNEL_READINGS() {
tnhnrl 52:f207567d3ea4 1879 char TUNING_key;
tnhnrl 52:f207567d3ea4 1880
tnhnrl 52:f207567d3ea4 1881 // show the menu
tnhnrl 58:94b7fd55185e 1882 pc().printf("\r\n8: CHANNEL READINGS (EXIT WITH 'X' !)");
tnhnrl 55:f4ec445c42fe 1883
tnhnrl 55:f4ec445c42fe 1884 while (1) {
tnhnrl 55:f4ec445c42fe 1885 if (pc().readable()) {
tnhnrl 55:f4ec445c42fe 1886 TUNING_key = pc().getc(); //get each keystroke
tnhnrl 55:f4ec445c42fe 1887 }
tnhnrl 55:f4ec445c42fe 1888
tnhnrl 55:f4ec445c42fe 1889 // process the keys
tnhnrl 55:f4ec445c42fe 1890 if (TUNING_key == 'X') {
tnhnrl 55:f4ec445c42fe 1891 // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
tnhnrl 55:f4ec445c42fe 1892 bce().pause();
tnhnrl 55:f4ec445c42fe 1893 batt().pause();
tnhnrl 55:f4ec445c42fe 1894
tnhnrl 55:f4ec445c42fe 1895 break; //exit the while loop
tnhnrl 55:f4ec445c42fe 1896 }
tnhnrl 55:f4ec445c42fe 1897
tnhnrl 55:f4ec445c42fe 1898 else {
tnhnrl 55:f4ec445c42fe 1899 wait(0.5);
tnhnrl 55:f4ec445c42fe 1900 pc().printf("0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7());
tnhnrl 55:f4ec445c42fe 1901 continue; // didn't get a user input, so keep waiting for it
tnhnrl 55:f4ec445c42fe 1902 }
tnhnrl 55:f4ec445c42fe 1903 }
tnhnrl 55:f4ec445c42fe 1904 }
tnhnrl 55:f4ec445c42fe 1905
tnhnrl 55:f4ec445c42fe 1906 void StateMachine::keyboard_menu_POSITION_READINGS() {
tnhnrl 55:f4ec445c42fe 1907 char TUNING_key;
tnhnrl 55:f4ec445c42fe 1908
tnhnrl 55:f4ec445c42fe 1909 // show the menu
tnhnrl 58:94b7fd55185e 1910 pc().printf("\r\n9: BCE and BMM POSITION READINGS (EXIT WITH 'X' !)");
tnhnrl 52:f207567d3ea4 1911
tnhnrl 52:f207567d3ea4 1912 while (1) {
tnhnrl 52:f207567d3ea4 1913 if (pc().readable()) {
tnhnrl 52:f207567d3ea4 1914 TUNING_key = pc().getc(); //get each keystroke
tnhnrl 52:f207567d3ea4 1915 }
tnhnrl 52:f207567d3ea4 1916
tnhnrl 54:d4990fb68404 1917 // process the keys
tnhnrl 54:d4990fb68404 1918 if (TUNING_key == 'X') {
tnhnrl 54:d4990fb68404 1919 // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
tnhnrl 54:d4990fb68404 1920 bce().pause();
tnhnrl 54:d4990fb68404 1921 batt().pause();
tnhnrl 54:d4990fb68404 1922
tnhnrl 54:d4990fb68404 1923 break; //exit the while loop
tnhnrl 54:d4990fb68404 1924 }
tnhnrl 54:d4990fb68404 1925
tnhnrl 52:f207567d3ea4 1926 else {
tnhnrl 52:f207567d3ea4 1927 // Testing out ADC
tnhnrl 54:d4990fb68404 1928 wait(0.5);
tnhnrl 52:f207567d3ea4 1929 float vref = 5.6;
tnhnrl 52:f207567d3ea4 1930 float vmeasured = 0;
tnhnrl 52:f207567d3ea4 1931 unsigned int raw = adc().readCh5();
tnhnrl 52:f207567d3ea4 1932 vmeasured = ((float)raw)/4095.0*vref;
tnhnrl 52:f207567d3ea4 1933
tnhnrl 54:d4990fb68404 1934
tnhnrl 55:f4ec445c42fe 1935 //pc().printf("BCE POS(%d),BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) \r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4());
tnhnrl 55:f4ec445c42fe 1936
tnhnrl 55:f4ec445c42fe 1937 pc().printf("BCE POS(%d), BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) << POS: BCE %0.2f, BATT %0.2f >>\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),bce().getPosition_mm(),batt().getPosition_mm());
tnhnrl 54:d4990fb68404 1938
tnhnrl 54:d4990fb68404 1939 //pc().printf("vessel pressure(%d) batt voltage(%d) board current(%d) LIMIT: BCE(%d) BMM(%d) (HW reading limit: BCE(%d) BMM (%d) \r\n",adc().readCh5(),adc().readCh6(),adc().readCh7(),bce().getSwitch(), batt().getSwitch(),bce().getSwitchState(),batt().getSwitchState());
tnhnrl 54:d4990fb68404 1940 //pc().printf("LIMIT: BCE(%d) BMM(%d) << HW reading limit: BCE(%d) BMM (%d) >> \r\n",bce().getSwitch(), batt().getSwitch(),bce().getHWSwitchReading(),batt().getHWSwitchReading());
tnhnrl 54:d4990fb68404 1941 //pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
tnhnrl 52:f207567d3ea4 1942 // End of ADC Test
tnhnrl 52:f207567d3ea4 1943
tnhnrl 54:d4990fb68404 1944 // pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
tnhnrl 54:d4990fb68404 1945 // pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
tnhnrl 54:d4990fb68404 1946 // pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
tnhnrl 54:d4990fb68404 1947 // pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
tnhnrl 54:d4990fb68404 1948 // pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
tnhnrl 54:d4990fb68404 1949 // pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
tnhnrl 54:d4990fb68404 1950 // pc().printf("raw battery voltage: %d \r\n",adc().readCh6());
tnhnrl 54:d4990fb68404 1951 // pc().printf("raw board current: %d \r\n",adc().readCh7());
tnhnrl 54:d4990fb68404 1952
tnhnrl 52:f207567d3ea4 1953 continue; // didn't get a user input, so keep waiting for it
tnhnrl 52:f207567d3ea4 1954 }
tnhnrl 52:f207567d3ea4 1955 }
tnhnrl 52:f207567d3ea4 1956 }
tnhnrl 49:47ffa4feb6db 1957
tnhnrl 16:3363b9f14913 1958 void StateMachine::keyboard_menu_BCE_PID_settings() {
tnhnrl 16:3363b9f14913 1959 char PID_key;
tnhnrl 63:6cb0405fc6e6 1960
tnhnrl 63:6cb0405fc6e6 1961 float bce_KP = bce().getControllerP(); // load current value
tnhnrl 63:6cb0405fc6e6 1962 float bce_KI = bce().getControllerI(); // load current global value
tnhnrl 63:6cb0405fc6e6 1963 float bce_KD = bce().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 1964
tnhnrl 16:3363b9f14913 1965 // show the menu
tnhnrl 63:6cb0405fc6e6 1966 pc().printf("\n\rBuoyancy Engine PID gain settings (MENU)");
tnhnrl 63:6cb0405fc6e6 1967 pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
tnhnrl 63:6cb0405fc6e6 1968 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
tnhnrl 63:6cb0405fc6e6 1969 pc().printf("bce P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
tnhnrl 63:6cb0405fc6e6 1970
tnhnrl 63:6cb0405fc6e6 1971 // handle the key presses
tnhnrl 63:6cb0405fc6e6 1972 while(1) {
tnhnrl 63:6cb0405fc6e6 1973 // get the user's keystroke from either of the two inputs
tnhnrl 63:6cb0405fc6e6 1974 if (pc().readable()) {
tnhnrl 63:6cb0405fc6e6 1975 PID_key = pc().getc();
tnhnrl 63:6cb0405fc6e6 1976 }
tnhnrl 63:6cb0405fc6e6 1977 else {
tnhnrl 63:6cb0405fc6e6 1978 continue; // didn't get a user input, so keep waiting for it
tnhnrl 63:6cb0405fc6e6 1979 }
tnhnrl 63:6cb0405fc6e6 1980
tnhnrl 63:6cb0405fc6e6 1981 // handle the user's key input
tnhnrl 63:6cb0405fc6e6 1982 if (PID_key == 'S') { // user wants to save these modified values
tnhnrl 63:6cb0405fc6e6 1983 // set values
tnhnrl 63:6cb0405fc6e6 1984 bce().setControllerP(bce_KP);
tnhnrl 63:6cb0405fc6e6 1985 bce().setControllerI(bce_KI);
tnhnrl 63:6cb0405fc6e6 1986 bce().setControllerD(bce_KD);
tnhnrl 63:6cb0405fc6e6 1987
tnhnrl 63:6cb0405fc6e6 1988 // save to "BATT.TXT" file
tnhnrl 63:6cb0405fc6e6 1989 configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD);
tnhnrl 63:6cb0405fc6e6 1990 pc().printf("bce P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
tnhnrl 63:6cb0405fc6e6 1991 }
tnhnrl 63:6cb0405fc6e6 1992 else if (PID_key == 'X') {
tnhnrl 63:6cb0405fc6e6 1993 break; //exit the while loop
tnhnrl 63:6cb0405fc6e6 1994 }
tnhnrl 63:6cb0405fc6e6 1995 else if (PID_key == 'P') {
tnhnrl 63:6cb0405fc6e6 1996 pc().printf(">> Type in proportional gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 1997 bce_KP = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 1998 }
tnhnrl 63:6cb0405fc6e6 1999 else if (PID_key == 'I') {
tnhnrl 63:6cb0405fc6e6 2000 pc().printf(">> Type in integral gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2001 bce_KI = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2002 }
tnhnrl 63:6cb0405fc6e6 2003 else if (PID_key == 'D') {
tnhnrl 63:6cb0405fc6e6 2004 pc().printf(">> Type in derivative gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2005 bce_KD = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2006 }
tnhnrl 63:6cb0405fc6e6 2007 else {
tnhnrl 63:6cb0405fc6e6 2008 pc().printf("\n\rBCE: [%c] This key does nothing here. \r", PID_key);
tnhnrl 63:6cb0405fc6e6 2009 }
tnhnrl 63:6cb0405fc6e6 2010 }
tnhnrl 63:6cb0405fc6e6 2011 }
tnhnrl 63:6cb0405fc6e6 2012
tnhnrl 63:6cb0405fc6e6 2013 void StateMachine::keyboard_menu_BATT_PID_settings() {
tnhnrl 63:6cb0405fc6e6 2014 char PID_key;
tnhnrl 63:6cb0405fc6e6 2015
tnhnrl 63:6cb0405fc6e6 2016 float batt_KP = batt().getControllerP(); // load current global value
tnhnrl 63:6cb0405fc6e6 2017 float batt_KI = batt().getControllerI(); // load current global value
tnhnrl 63:6cb0405fc6e6 2018 float batt_KD = batt().getControllerD(); // load current global value
tnhnrl 63:6cb0405fc6e6 2019
tnhnrl 63:6cb0405fc6e6 2020 // print the menu
tnhnrl 63:6cb0405fc6e6 2021 pc().printf("\n\rBattery Motor PID gain settings (MENU)");
tnhnrl 63:6cb0405fc6e6 2022 pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
tnhnrl 63:6cb0405fc6e6 2023 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
tnhnrl 63:6cb0405fc6e6 2024 pc().printf("batt P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
tnhnrl 16:3363b9f14913 2025
tnhnrl 16:3363b9f14913 2026 // handle the key presses
tnhnrl 16:3363b9f14913 2027 while(1) {
tnhnrl 16:3363b9f14913 2028 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 2029 if (pc().readable()) {
tnhnrl 16:3363b9f14913 2030 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 2031 }
tnhnrl 16:3363b9f14913 2032 else {
tnhnrl 16:3363b9f14913 2033 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 2034 }
tnhnrl 16:3363b9f14913 2035
tnhnrl 16:3363b9f14913 2036 // handle the user's key input
tnhnrl 63:6cb0405fc6e6 2037 if (PID_key == 'S') { // user wants to save these modified values
tnhnrl 16:3363b9f14913 2038 // set values
tnhnrl 63:6cb0405fc6e6 2039 batt().setControllerP(batt_KP);
tnhnrl 63:6cb0405fc6e6 2040 batt().setControllerI(batt_KI);
tnhnrl 63:6cb0405fc6e6 2041 batt().setControllerD(batt_KD);
tnhnrl 16:3363b9f14913 2042
tnhnrl 38:83d06c294807 2043 // save to "BATT.TXT" file
tnhnrl 63:6cb0405fc6e6 2044 configFileIO().saveBattData(batt_KP, batt_KI, batt_KD);
tnhnrl 63:6cb0405fc6e6 2045 pc().printf("batt P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
tnhnrl 16:3363b9f14913 2046 }
tnhnrl 16:3363b9f14913 2047 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 2048 break; //exit the while loop
tnhnrl 16:3363b9f14913 2049 }
tnhnrl 63:6cb0405fc6e6 2050 else if (PID_key == 'P') {
tnhnrl 63:6cb0405fc6e6 2051 pc().printf(">> Type in proportional gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2052 batt_KP = getFloatUserInput();
tnhnrl 16:3363b9f14913 2053 }
tnhnrl 63:6cb0405fc6e6 2054 else if (PID_key == 'I') {
tnhnrl 63:6cb0405fc6e6 2055 pc().printf(">> Type in integral gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2056 batt_KI = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2057 }
tnhnrl 63:6cb0405fc6e6 2058 else if (PID_key == 'D') {
tnhnrl 63:6cb0405fc6e6 2059 pc().printf(">> Type in derivative gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2060 batt_KD = getFloatUserInput();
tnhnrl 16:3363b9f14913 2061 }
tnhnrl 16:3363b9f14913 2062 else {
tnhnrl 63:6cb0405fc6e6 2063 pc().printf("\n\rBATT: [%c] This key does nothing here. \r", PID_key);
tnhnrl 16:3363b9f14913 2064 }
tnhnrl 16:3363b9f14913 2065 }
tnhnrl 16:3363b9f14913 2066 }
tnhnrl 20:8987a9ae2bc7 2067
tnhnrl 16:3363b9f14913 2068 void StateMachine::keyboard_menu_DEPTH_PID_settings() {
tnhnrl 16:3363b9f14913 2069 char PID_key;
tnhnrl 63:6cb0405fc6e6 2070
tnhnrl 63:6cb0405fc6e6 2071 float depth_KP = depthLoop().getControllerP(); // load current depth value
tnhnrl 63:6cb0405fc6e6 2072 float depth_KI = depthLoop().getControllerI(); // load current depth value
tnhnrl 63:6cb0405fc6e6 2073 float depth_KD = depthLoop().getControllerD(); // load current depth value
tnhnrl 16:3363b9f14913 2074
tnhnrl 63:6cb0405fc6e6 2075 // print the menu
tnhnrl 63:6cb0405fc6e6 2076 pc().printf("\n\rDEPTH (Buoyancy Engine O.L.) PID gain settings (MENU)");
tnhnrl 63:6cb0405fc6e6 2077 pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
tnhnrl 63:6cb0405fc6e6 2078 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
tnhnrl 63:6cb0405fc6e6 2079 pc().printf("DEPTH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 2080
tnhnrl 16:3363b9f14913 2081 // handle the key presses
tnhnrl 16:3363b9f14913 2082 while(1) {
tnhnrl 16:3363b9f14913 2083 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 2084 if (pc().readable()) {
tnhnrl 16:3363b9f14913 2085 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 2086 }
tnhnrl 16:3363b9f14913 2087 else {
tnhnrl 16:3363b9f14913 2088 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 2089 }
tnhnrl 16:3363b9f14913 2090
tnhnrl 16:3363b9f14913 2091 // handle the user's key input
tnhnrl 63:6cb0405fc6e6 2092 if (PID_key == 'S') { // user wants to save these modified values
tnhnrl 63:6cb0405fc6e6 2093 // set values
tnhnrl 63:6cb0405fc6e6 2094 depthLoop().setControllerP(depth_KP);
tnhnrl 63:6cb0405fc6e6 2095 depthLoop().setControllerI(depth_KI);
tnhnrl 63:6cb0405fc6e6 2096 depthLoop().setControllerD(depth_KD);
tnhnrl 63:6cb0405fc6e6 2097
tnhnrl 63:6cb0405fc6e6 2098 // save to "DEPTH.TXT" file
tnhnrl 63:6cb0405fc6e6 2099 configFileIO().saveDepthData(depth_KP, depth_KI, depth_KD, _neutral_bce_pos_mm);
tnhnrl 63:6cb0405fc6e6 2100 pc().printf("DEPTH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 2101
tnhnrl 63:6cb0405fc6e6 2102 //set class variables that will be used in find neutral sequence
tnhnrl 63:6cb0405fc6e6 2103 _depth_KP = depthLoop().getControllerP(); // load current depth value
tnhnrl 63:6cb0405fc6e6 2104 _depth_KI = depthLoop().getControllerI(); // load current depth value
tnhnrl 63:6cb0405fc6e6 2105 _depth_KD = depthLoop().getControllerD(); // load current depth value
tnhnrl 16:3363b9f14913 2106 }
tnhnrl 16:3363b9f14913 2107 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 2108 break; //exit the while loop
tnhnrl 16:3363b9f14913 2109 }
tnhnrl 63:6cb0405fc6e6 2110 else if (PID_key == 'P') {
tnhnrl 63:6cb0405fc6e6 2111 pc().printf(">> Type in proportional gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2112 depth_KP = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2113 }
tnhnrl 63:6cb0405fc6e6 2114 else if (PID_key == 'I') {
tnhnrl 63:6cb0405fc6e6 2115 pc().printf(">> Type in integral gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2116 depth_KI = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2117 }
tnhnrl 63:6cb0405fc6e6 2118 else if (PID_key == 'D') {
tnhnrl 63:6cb0405fc6e6 2119 pc().printf(">> Type in derivative gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2120 depth_KD = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2121 }
tnhnrl 16:3363b9f14913 2122 else {
tnhnrl 63:6cb0405fc6e6 2123 pc().printf("\n\rDEPTH: [%c] This key does nothing here. \r", PID_key);
tnhnrl 16:3363b9f14913 2124 }
tnhnrl 16:3363b9f14913 2125 }
tnhnrl 16:3363b9f14913 2126 }
tnhnrl 16:3363b9f14913 2127
tnhnrl 16:3363b9f14913 2128 void StateMachine::keyboard_menu_PITCH_PID_settings() {
tnhnrl 16:3363b9f14913 2129 char PID_key;
tnhnrl 63:6cb0405fc6e6 2130
tnhnrl 63:6cb0405fc6e6 2131 float pitch_KP = pitchLoop().getControllerP(); // load current pitch value
tnhnrl 63:6cb0405fc6e6 2132 float pitch_KI = pitchLoop().getControllerI(); // load current pitch value
tnhnrl 63:6cb0405fc6e6 2133 float pitch_KD = pitchLoop().getControllerD(); // load current pitch value
tnhnrl 16:3363b9f14913 2134
tnhnrl 16:3363b9f14913 2135 // print the menu
tnhnrl 63:6cb0405fc6e6 2136 pc().printf("\n\rPITCH (Battery Motor O.L.) PID gain settings (MENU)");
tnhnrl 63:6cb0405fc6e6 2137 pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
tnhnrl 63:6cb0405fc6e6 2138 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
tnhnrl 63:6cb0405fc6e6 2139 pc().printf("PITCH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
tnhnrl 63:6cb0405fc6e6 2140
tnhnrl 16:3363b9f14913 2141 // handle the key presses
tnhnrl 16:3363b9f14913 2142 while(1) {
tnhnrl 16:3363b9f14913 2143 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 2144 if (pc().readable()) {
tnhnrl 16:3363b9f14913 2145 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 2146 }
tnhnrl 16:3363b9f14913 2147 else {
tnhnrl 16:3363b9f14913 2148 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 2149 }
tnhnrl 63:6cb0405fc6e6 2150
tnhnrl 16:3363b9f14913 2151 // handle the user's key input
tnhnrl 63:6cb0405fc6e6 2152 if (PID_key == 'S') { // user wants to save these modified values
tnhnrl 63:6cb0405fc6e6 2153 // set values
tnhnrl 63:6cb0405fc6e6 2154 pitchLoop().setControllerP(pitch_KP);
tnhnrl 63:6cb0405fc6e6 2155 pitchLoop().setControllerI(pitch_KI);
tnhnrl 63:6cb0405fc6e6 2156 pitchLoop().setControllerD(pitch_KD);
tnhnrl 63:6cb0405fc6e6 2157
tnhnrl 63:6cb0405fc6e6 2158 // save to "PITCH.TXT" file
tnhnrl 63:6cb0405fc6e6 2159 configFileIO().savePitchData(pitch_KP, pitch_KI, pitch_KD, _neutral_batt_pos_mm);
tnhnrl 63:6cb0405fc6e6 2160 pc().printf("PITCH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
tnhnrl 63:6cb0405fc6e6 2161
tnhnrl 63:6cb0405fc6e6 2162 _pitch_KP = pitchLoop().getControllerP(); // load current pitch value
tnhnrl 63:6cb0405fc6e6 2163 _pitch_KI = pitchLoop().getControllerI(); // load current pitch value
tnhnrl 63:6cb0405fc6e6 2164 _pitch_KD = pitchLoop().getControllerD(); // load current pitch value
tnhnrl 16:3363b9f14913 2165 }
tnhnrl 16:3363b9f14913 2166 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 2167 break; //exit the while loop
tnhnrl 16:3363b9f14913 2168 }
tnhnrl 63:6cb0405fc6e6 2169 else if (PID_key == 'P') {
tnhnrl 63:6cb0405fc6e6 2170 pc().printf(">> Type in proportional gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2171 pitch_KP = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2172 }
tnhnrl 63:6cb0405fc6e6 2173 else if (PID_key == 'I') {
tnhnrl 63:6cb0405fc6e6 2174 pc().printf(">> Type in integral gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2175 pitch_KI = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2176 }
tnhnrl 63:6cb0405fc6e6 2177 else if (PID_key == 'D') {
tnhnrl 63:6cb0405fc6e6 2178 pc().printf(">> Type in derivative gain with keyboard.\n\r");
tnhnrl 63:6cb0405fc6e6 2179 pitch_KD = getFloatUserInput();
tnhnrl 63:6cb0405fc6e6 2180 }
tnhnrl 16:3363b9f14913 2181 else {
tnhnrl 63:6cb0405fc6e6 2182 pc().printf("\n\rPITCH: [%c] This key does nothing here. \r", PID_key);
tnhnrl 16:3363b9f14913 2183 }
tnhnrl 16:3363b9f14913 2184 }
tnhnrl 16:3363b9f14913 2185 }
tnhnrl 20:8987a9ae2bc7 2186
tnhnrl 16:3363b9f14913 2187 float StateMachine::getDepthCommand() {
tnhnrl 32:f2f8ae34aadc 2188 return _depth_command;
tnhnrl 16:3363b9f14913 2189 }
tnhnrl 20:8987a9ae2bc7 2190
tnhnrl 16:3363b9f14913 2191 float StateMachine::getPitchCommand() {
tnhnrl 32:f2f8ae34aadc 2192 return _pitch_command;
tnhnrl 32:f2f8ae34aadc 2193 }
tnhnrl 32:f2f8ae34aadc 2194
tnhnrl 32:f2f8ae34aadc 2195 float StateMachine::getDepthReading() {
tnhnrl 32:f2f8ae34aadc 2196 return _depth_reading;
tnhnrl 32:f2f8ae34aadc 2197 }
tnhnrl 32:f2f8ae34aadc 2198
tnhnrl 32:f2f8ae34aadc 2199 float StateMachine::getPitchReading() {
tnhnrl 32:f2f8ae34aadc 2200 return _pitch_reading;
tnhnrl 32:f2f8ae34aadc 2201 }
tnhnrl 32:f2f8ae34aadc 2202
tnhnrl 32:f2f8ae34aadc 2203 float StateMachine::getTimerReading() {
tnhnrl 32:f2f8ae34aadc 2204 return _timer_reading;
tnhnrl 17:7c16b5671d0e 2205 }
tnhnrl 28:16c83a2fdefa 2206
tnhnrl 17:7c16b5671d0e 2207 void StateMachine::setState(int input_state) {
tnhnrl 21:38c8544db6f4 2208 _state = input_state;
tnhnrl 17:7c16b5671d0e 2209 }
tnhnrl 20:8987a9ae2bc7 2210
tnhnrl 17:7c16b5671d0e 2211 int StateMachine::getState() {
tnhnrl 17:7c16b5671d0e 2212 return _state; //return the current state of the system
tnhnrl 17:7c16b5671d0e 2213 }
tnhnrl 20:8987a9ae2bc7 2214
tnhnrl 17:7c16b5671d0e 2215 void StateMachine::setTimeout(float input_timeout) {
tnhnrl 17:7c16b5671d0e 2216 _timeout = input_timeout;
tnhnrl 17:7c16b5671d0e 2217 }
tnhnrl 20:8987a9ae2bc7 2218
tnhnrl 17:7c16b5671d0e 2219 void StateMachine::setDepthCommand(float input_depth_command) {
tnhnrl 32:f2f8ae34aadc 2220 _depth_command = input_depth_command;
tnhnrl 17:7c16b5671d0e 2221 }
tnhnrl 20:8987a9ae2bc7 2222
tnhnrl 17:7c16b5671d0e 2223 void StateMachine::setPitchCommand(float input_pitch_command) {
tnhnrl 32:f2f8ae34aadc 2224 _pitch_command = input_pitch_command;
tnhnrl 17:7c16b5671d0e 2225 }
tnhnrl 20:8987a9ae2bc7 2226
tnhnrl 17:7c16b5671d0e 2227 void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) {
tnhnrl 21:38c8544db6f4 2228 _neutral_batt_pos_mm = batt_pos_mm;
tnhnrl 21:38c8544db6f4 2229 _neutral_bce_pos_mm = bce_pos_mm;
tnhnrl 17:7c16b5671d0e 2230
tnhnrl 58:94b7fd55185e 2231 pc().printf("Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\r\n",_neutral_batt_pos_mm,_neutral_bce_pos_mm);
tnhnrl 17:7c16b5671d0e 2232 }
tnhnrl 20:8987a9ae2bc7 2233
tnhnrl 17:7c16b5671d0e 2234 int StateMachine::timeoutRunning() {
tnhnrl 28:16c83a2fdefa 2235 return _isTimeoutRunning;
tnhnrl 17:7c16b5671d0e 2236 }
tnhnrl 20:8987a9ae2bc7 2237
tnhnrl 17:7c16b5671d0e 2238 //process one state at a time
tnhnrl 17:7c16b5671d0e 2239 void StateMachine::getDiveSequence() {
tnhnrl 17:7c16b5671d0e 2240 //iterate through this sequence using the FSM
tnhnrl 24:c7d9b5bf3829 2241 currentStateStruct.state = sequenceController().sequenceStructLoaded[_multi_dive_counter].state;
tnhnrl 24:c7d9b5bf3829 2242 currentStateStruct.timeout = sequenceController().sequenceStructLoaded[_multi_dive_counter].timeout;
tnhnrl 24:c7d9b5bf3829 2243 currentStateStruct.depth = sequenceController().sequenceStructLoaded[_multi_dive_counter].depth;
tnhnrl 24:c7d9b5bf3829 2244 currentStateStruct.pitch = sequenceController().sequenceStructLoaded[_multi_dive_counter].pitch;
tnhnrl 17:7c16b5671d0e 2245
tnhnrl 17:7c16b5671d0e 2246 _timeout = currentStateStruct.timeout; //set timeout before exiting this function
tnhnrl 32:f2f8ae34aadc 2247 }
tnhnrl 32:f2f8ae34aadc 2248
tnhnrl 32:f2f8ae34aadc 2249 void StateMachine::printCurrentSdLog() {
tnhnrl 58:94b7fd55185e 2250 pc().printf("SD card log work in progress\r\n");
tnhnrl 32:f2f8ae34aadc 2251 //might be worth saving the last few logs to the MBED...
tnhnrl 32:f2f8ae34aadc 2252 }
tnhnrl 32:f2f8ae34aadc 2253
tnhnrl 32:f2f8ae34aadc 2254 //check if the file is still opened
tnhnrl 32:f2f8ae34aadc 2255 void StateMachine::createNewFile() {
tnhnrl 32:f2f8ae34aadc 2256 if (_file_closed) {
tnhnrl 34:9b66c5188051 2257 //mbedLogger().createFile(); //create a new MBED file
tnhnrl 32:f2f8ae34aadc 2258
tnhnrl 32:f2f8ae34aadc 2259 _file_closed = false; //file is still open until you get to SIT_IDLE
tnhnrl 32:f2f8ae34aadc 2260 }
tnhnrl 32:f2f8ae34aadc 2261 }
tnhnrl 32:f2f8ae34aadc 2262
tnhnrl 32:f2f8ae34aadc 2263 void StateMachine::transmitData() {
tnhnrl 32:f2f8ae34aadc 2264 static float transmit_timer = 0;
tnhnrl 32:f2f8ae34aadc 2265 static bool is_transmit_timer_running = false;
tnhnrl 32:f2f8ae34aadc 2266
tnhnrl 32:f2f8ae34aadc 2267 if (!is_transmit_timer_running) {
tnhnrl 58:94b7fd55185e 2268 //pc().printf("\r\n\nTRANSMIT timer running...\r\n\n"); //debug
tnhnrl 32:f2f8ae34aadc 2269
tnhnrl 32:f2f8ae34aadc 2270 transmit_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds
tnhnrl 32:f2f8ae34aadc 2271 is_transmit_timer_running = true; //disable this block after one iteration
tnhnrl 32:f2f8ae34aadc 2272
tnhnrl 58:94b7fd55185e 2273 pc().printf("TESTING to see if this transmits once a second. (timer: %0.1f)\r\n", timer.read());
tnhnrl 32:f2f8ae34aadc 2274 }
tnhnrl 32:f2f8ae34aadc 2275 if (timer.read() >= transmit_timer) {
tnhnrl 32:f2f8ae34aadc 2276 is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 32:f2f8ae34aadc 2277 }
tnhnrl 34:9b66c5188051 2278 }
tnhnrl 34:9b66c5188051 2279
tnhnrl 34:9b66c5188051 2280 void StateMachine::recordData(int input_state) {
tnhnrl 34:9b66c5188051 2281 string string_state;
tnhnrl 34:9b66c5188051 2282 if (input_state == SIT_IDLE)
tnhnrl 34:9b66c5188051 2283 string_state = "SIT_IDLE";
tnhnrl 34:9b66c5188051 2284 else if (input_state == FIND_NEUTRAL)
tnhnrl 34:9b66c5188051 2285 string_state = "FIND_NEUTRAL";
tnhnrl 34:9b66c5188051 2286 else if (input_state == DIVE)
tnhnrl 34:9b66c5188051 2287 string_state = "DIVE";
tnhnrl 34:9b66c5188051 2288 else if (input_state == RISE)
tnhnrl 34:9b66c5188051 2289 string_state = "RISE";
tnhnrl 34:9b66c5188051 2290 else if (input_state == FLOAT_LEVEL)
tnhnrl 34:9b66c5188051 2291 string_state = "FLOAT_LEVEL";
tnhnrl 34:9b66c5188051 2292 else if (input_state == FLOAT_BROADCAST)
tnhnrl 34:9b66c5188051 2293 string_state = "FLOAT_BROADCAST";
tnhnrl 34:9b66c5188051 2294 else if (input_state == EMERGENCY_CLIMB)
tnhnrl 34:9b66c5188051 2295 string_state = "EMERGENCY_CLIMB";
tnhnrl 34:9b66c5188051 2296 else if (input_state == MULTI_DIVE)
tnhnrl 34:9b66c5188051 2297 string_state = "MULTI_DIVE";
tnhnrl 34:9b66c5188051 2298 else if (input_state == MULTI_RISE)
tnhnrl 34:9b66c5188051 2299 string_state = "MULTI_RISE";
tnhnrl 34:9b66c5188051 2300 else if (input_state == KEYBOARD)
tnhnrl 34:9b66c5188051 2301 string_state = "KEYBOARD";
tnhnrl 34:9b66c5188051 2302
tnhnrl 34:9b66c5188051 2303 if (!_is_log_timer_running) {
tnhnrl 58:94b7fd55185e 2304 //pc().printf("\r\n\nlog timer running...\r\n\n"); //debug
tnhnrl 34:9b66c5188051 2305
tnhnrl 34:9b66c5188051 2306 _log_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds
tnhnrl 34:9b66c5188051 2307 _is_log_timer_running = true; //disable this block after one iteration
tnhnrl 34:9b66c5188051 2308
tnhnrl 37:357e98a929cc 2309 _data_log[0] = systemTime().read(); //system time reading
tnhnrl 34:9b66c5188051 2310 _data_log[1] = depthLoop().getCommand(); //depth command
tnhnrl 34:9b66c5188051 2311 _data_log[2] = depthLoop().getPosition(); //depth reading
tnhnrl 34:9b66c5188051 2312 _data_log[3] = pitchLoop().getCommand(); //pitch command
tnhnrl 34:9b66c5188051 2313 _data_log[4] = pitchLoop().getPosition(); //pitch reading
tnhnrl 34:9b66c5188051 2314 _data_log[5] = bce().getSetPosition_mm();
tnhnrl 34:9b66c5188051 2315 _data_log[6] = bce().getPosition_mm();
tnhnrl 34:9b66c5188051 2316 _data_log[7] = batt().getSetPosition_mm();
tnhnrl 34:9b66c5188051 2317 _data_log[8] = batt().getPosition_mm();
tnhnrl 34:9b66c5188051 2318
tnhnrl 34:9b66c5188051 2319 //record data to the MBED every 5 seconds
tnhnrl 35:2f66ea4863d5 2320 //mbedLogger().saveArrayToFile(string_state,input_state,_data_log);
tnhnrl 34:9b66c5188051 2321 }
tnhnrl 34:9b66c5188051 2322 if (timer.read() >= _log_timer) {
tnhnrl 34:9b66c5188051 2323 _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
tnhnrl 34:9b66c5188051 2324 }
tnhnrl 34:9b66c5188051 2325 }
tnhnrl 34:9b66c5188051 2326
tnhnrl 34:9b66c5188051 2327 void StateMachine::recordState(int input_state) {
tnhnrl 34:9b66c5188051 2328 string string_state;
tnhnrl 34:9b66c5188051 2329
tnhnrl 34:9b66c5188051 2330 if (input_state == SIT_IDLE)
tnhnrl 34:9b66c5188051 2331 string_state = "SIT_IDLE";
tnhnrl 34:9b66c5188051 2332 else if (input_state == FIND_NEUTRAL)
tnhnrl 34:9b66c5188051 2333 string_state = "FIND_NEUTRAL";
tnhnrl 34:9b66c5188051 2334 else if (input_state == DIVE)
tnhnrl 34:9b66c5188051 2335 string_state = "DIVE";
tnhnrl 34:9b66c5188051 2336 else if (input_state == RISE)
tnhnrl 34:9b66c5188051 2337 string_state = "RISE";
tnhnrl 34:9b66c5188051 2338 else if (input_state == FLOAT_LEVEL)
tnhnrl 34:9b66c5188051 2339 string_state = "FLOAT_LEVEL";
tnhnrl 34:9b66c5188051 2340 else if (input_state == FLOAT_BROADCAST)
tnhnrl 34:9b66c5188051 2341 string_state = "FLOAT_BROADCAST";
tnhnrl 34:9b66c5188051 2342 else if (input_state == EMERGENCY_CLIMB)
tnhnrl 34:9b66c5188051 2343 string_state = "EMERGENCY_CLIMB";
tnhnrl 34:9b66c5188051 2344 else if (input_state == MULTI_DIVE)
tnhnrl 34:9b66c5188051 2345 string_state = "MULTI_DIVE";
tnhnrl 34:9b66c5188051 2346 else if (input_state == MULTI_RISE)
tnhnrl 34:9b66c5188051 2347 string_state = "MULTI_RISE";
tnhnrl 34:9b66c5188051 2348 else if (input_state == KEYBOARD)
tnhnrl 34:9b66c5188051 2349 string_state = "KEYBOARD";
tnhnrl 34:9b66c5188051 2350 //datalogger().printf("%s\n", string_state.c_str());
tnhnrl 36:966a86937e17 2351 }
tnhnrl 36:966a86937e17 2352
tnhnrl 36:966a86937e17 2353 float * StateMachine::dataArray() {
tnhnrl 36:966a86937e17 2354 //return the array to a calling function
tnhnrl 36:966a86937e17 2355 return _data_log;
tnhnrl 52:f207567d3ea4 2356 }
tnhnrl 52:f207567d3ea4 2357
tnhnrl 52:f207567d3ea4 2358 // 06/06/2018
tnhnrl 52:f207567d3ea4 2359 float StateMachine::getFloatUserInput() {
tnhnrl 52:f207567d3ea4 2360 float float_conversion = 0.0;
tnhnrl 52:f207567d3ea4 2361
tnhnrl 52:f207567d3ea4 2362 while(1) {
tnhnrl 52:f207567d3ea4 2363 bool valid_input = false; //flag for valid or invalid input
tnhnrl 52:f207567d3ea4 2364
tnhnrl 58:94b7fd55185e 2365 pc().printf("\n\rPlease enter your number below and press ENTER:\r\n");
tnhnrl 52:f207567d3ea4 2366 char user_string [80]; //variable to store input as a character array
tnhnrl 52:f207567d3ea4 2367
tnhnrl 52:f207567d3ea4 2368 pc().scanf("%s", user_string); //read formatted data from stdin
tnhnrl 58:94b7fd55185e 2369 pc().printf("\n\n\ruser_string was <%s>\r\n", user_string);
tnhnrl 52:f207567d3ea4 2370
tnhnrl 52:f207567d3ea4 2371 //check through the string for invalid characters (decimal values 43 through 57)
tnhnrl 52:f207567d3ea4 2372 for (int c = 0; c < strlen(user_string); c++) {
tnhnrl 58:94b7fd55185e 2373 //pc().printf("character is [%c]\r\n", user_string[c]); //debug
tnhnrl 52:f207567d3ea4 2374 if (user_string[c] >= 43 and user_string[c] <= 57) {
tnhnrl 58:94b7fd55185e 2375 //pc().printf("VALID CHARACTER!\r\n"); //debug
tnhnrl 52:f207567d3ea4 2376 ;
tnhnrl 52:f207567d3ea4 2377 }
tnhnrl 52:f207567d3ea4 2378 else {
tnhnrl 58:94b7fd55185e 2379 pc().printf("INVALID INPUT!\r\n");
tnhnrl 52:f207567d3ea4 2380 break;
tnhnrl 52:f207567d3ea4 2381 }
tnhnrl 52:f207567d3ea4 2382
tnhnrl 52:f207567d3ea4 2383 if (c == (strlen(user_string) - 1)) {
tnhnrl 52:f207567d3ea4 2384 valid_input = true;
tnhnrl 52:f207567d3ea4 2385 }
tnhnrl 52:f207567d3ea4 2386 }
tnhnrl 52:f207567d3ea4 2387
tnhnrl 52:f207567d3ea4 2388 if (valid_input) {
tnhnrl 52:f207567d3ea4 2389 float_conversion = atof(user_string);
tnhnrl 63:6cb0405fc6e6 2390 pc().printf("VALID INPUT! Your input was: %3.3f (PRESS \"S\" (shift + S) to save!)\r\n", float_conversion);
tnhnrl 52:f207567d3ea4 2391 break;
tnhnrl 52:f207567d3ea4 2392 }
tnhnrl 52:f207567d3ea4 2393 }
tnhnrl 52:f207567d3ea4 2394
tnhnrl 52:f207567d3ea4 2395 return float_conversion;
tnhnrl 16:3363b9f14913 2396 }