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@73:f6f378311c8d, 2018-07-30 (annotated)

Committer:
tnhnrl
Date:
Mon Jul 30 16:48:48 2018 +0000
Revision:
73:f6f378311c8d
Parent:
72:250b2665755c
Child:
74:d281aaef9766
work in progress 7/30 12:48 pm

Who changed what in which revision?

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