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@76:c802e1da4179, 2018-08-16 (annotated)

Committer:
tnhnrl
Date:
Thu Aug 16 21:47:40 2018 +0000
Revision:
76:c802e1da4179
Parent:
74:d281aaef9766
Child:
80:4e5d306d695b
Stable version of code with PID zero offset save fixed!

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