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@85:dd8176285b6e, 2019-02-25 (annotated)

Committer:
joel_ssc
Date:
Mon Feb 25 21:26:34 2019 +0000
Revision:
85:dd8176285b6e
Parent:
84:eccd8e837134
Child:
87:6d95f853dab3
tests imu.roll

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