2019_13sep_jcw_nosd - update with altimeter, swimfile.txt endleg.txt, e…

Flying Sea Glider » Code » 2019_13sep_jcw_nosd

Flying Sea Glider / Mbed 2 deprecated 2019_13sep_jcw_nosd

update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()

Dependencies: mbed MODSERIAL FATFileSystem

StateMachine/StateMachine.cpp@97:2b4f78a54227, 2019-05-17 (annotated)

Committer:: joel_ssc
Date:: Fri May 17 20:43:43 2019 +0000
Revision:: 97:2b4f78a54227
Parent:: 95:1aac4086928a
Child:: 99:9d0849f5fcd7
Child:: 102:0f430de62447

adjusted logic in main.cpp to go to start_swim if neutral not yet found

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	13 Sep 2019
Imports:	0
Forks:	0
Commits:	105
Dependents:	0
Dependencies:	3
Followers:	10

StateMachine/StateMachine.cpp@97:2b4f78a54227, 2019-05-17 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning