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@56:48a8a5a65b82, 2018-06-12 (annotated)

Committer:
tnhnrl
Date:
Tue Jun 12 16:20:03 2018 +0000
Revision:
56:48a8a5a65b82
Parent:
55:f4ec445c42fe
Child:
57:ec69651c8c21
good stuff, rudder and everything.; ; Need to fix heading next.

Who changed what in which revision?

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