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@33:29a4268fbc74, 2017-12-20 (annotated)

Committer:
tnhnrl
Date:
Wed Dec 20 14:57:18 2017 +0000
Revision:
33:29a4268fbc74
Parent:
32:f2f8ae34aadc 
revb need to fix rtos timer

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