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@104:426224a55f5f, 2019-09-13 (annotated)

Committer:: joel_ssc
Date:: Fri Sep 13 19:15:40 2019 +0000
Revision:: 104:426224a55f5f
Parent:: 102:0f430de62447

slight change  includes reset_PI

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

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@104:426224a55f5f, 2019-09-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning