Flying Sea Glider
most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
Diff: StateMachine/StateMachine.cpp
- Revision:
- 67:c86a4b464682
- Parent:
- 66:0f20870117b7
- Child:
- 68:8f549749b8ce
--- a/StateMachine/StateMachine.cpp Tue Jun 19 20:14:23 2018 +0000
+++ b/StateMachine/StateMachine.cpp Mon Jun 25 15:44:00 2018 +0000
@@ -65,7 +65,7 @@
//Finite State Machine (FSM)
int StateMachine::runStateMachine() {
static int transmit_packet_number = 1; //for data transmission
-
+
// finite state machine ... each state has at least one exit criteria
switch (_state) {
case SIT_IDLE :
@@ -115,13 +115,9 @@
pc().printf("CHECK_TUNING: BCE cmd: %3.1f (BCE current position: %3.1f)\r\n", bce().getSetPosition_mm(), bce().getPosition_mm());
pc().printf("CHECK_TUNING: BATT cmd: %3.1f (BATT current position: %3.1f)\r\n", batt().getSetPosition_mm(), bce().getPosition_mm());
-
- //show that this is the start of new dive sequence
- recordState(_state);
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -132,11 +128,13 @@
_isTimeoutRunning = false;
}
+ //WHAT IS ACTIVE?
+ // the inner loop position controls are maintaining the positions of the linear actuators
+
//print status to screen continuously
pc().printf("CHECK_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (BCE_cmd: %0.1f, BATT_cmd: %0.1f)(depth: %0.1f ft, pitch: %0.1f deg) [%0.1f sec]\r",bce().getPosition_mm(),batt().getPosition_mm(),bce().getSetPosition_mm(),batt().getSetPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(),timer.read());
- //record data every 5 seconds
- recordData(_state);
+ checkMotorPositions();
break;
@@ -154,17 +152,10 @@
// what are the commands?
bce().setPosition_mm(bce().getTravelLimit());
- batt().setPosition_mm(0.0);
-
- //create the log file (works only if the file is closed)
- ////////createNewFile();
-
- //show that this is the start of a new EMERGENCY_CLIMB sequence
- recordState(_state);
+ batt().setPosition_mm(10.0); //pull nose up (0.0 was sketchy)
//triggers logger array
_is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -180,11 +171,9 @@
_isTimeoutRunning = false;
}
+ //WHAT IS ACTIVE?
//print status to screen continuously
- pc().printf("EC: depth: %3.1f (BCE_cmd: %0.1f), pitch: %0.1f deg [%0.1f sec]\r",depthLoop().getPosition(),bce().getPosition_mm(),pitchLoop().getPosition(),timer.read());
-
- //record data every 5 seconds
- recordData(_state);
+ pc().printf("EC: depth: %3.1f, pitch: %0.1f deg [BCE:%0.1f (cmd: %0.1f) BMM:%0.1f (cmd: %0.1f)] [%0.1f sec]\r",depthLoop().getPosition(),pitchLoop().getPosition(),bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),timer.read());
break;
@@ -212,16 +201,9 @@
_substate_array_counter++;
runNeutralStateMachine();
-
- //create the log file (works only if the file is closed)
- //createNewFile();
-
- //show that this is the start of a new FIND_NEUTRAL sequence
- recordState(_state);
-
+
//triggers logger array
_is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit? (exit with the timer, if timer still running continue processing sub FSM)
@@ -245,9 +227,6 @@
_isTimeoutRunning = false;
}
- //record data every 5 seconds
- recordData(_state);
-
break;
case DIVE :
@@ -276,12 +255,8 @@
//create the log file (works only if the file is closed)
//createNewFile();
- //show that this is the start of new dive sequence
- recordState(_state);
-
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -298,7 +273,7 @@
_isTimeoutRunning = false;
}
- // what is active?
+ // WHAT IS ACTIVE?
pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depth: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read(),bce().getSetPosition_mm(),batt().getSetPosition_mm());
bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
batt().setPosition_mm(pitchLoop().getOutput());
@@ -307,8 +282,7 @@
_max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded
}
- //record data every 5 seconds
- recordData(_state);
+ checkMotorPositions();
break;
@@ -335,12 +309,8 @@
//create the log file (works only if the file is closed)
//createNewFile();
- //show that this is the start of new rise sequence
- recordState(_state);
-
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -360,13 +330,12 @@
_isTimeoutRunning = false;
}
- // what is active?
+ // WHAT IS ACTIVE?
pc().printf("RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read(),bce().getSetPosition_mm(),batt().getSetPosition_mm());
bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
batt().setPosition_mm(pitchLoop().getOutput());
-
- //record data every 5 seconds
- recordData(_state);
+
+ checkMotorPositions();
break;
@@ -399,12 +368,8 @@
//reset max dive depth
_max_recorded_depth_dive = -99; //float to record max depth
- //show that this is the start of new dive sequence
- recordState(_state);
-
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -434,8 +399,7 @@
// ACTIVE RUDDER CONTROL
rudder().setPosition_deg(headingLoop().getOutput());
- //record data internally (for access by MbedLogger)
- recordData(_state);
+ checkMotorPositions();
break;
@@ -465,12 +429,8 @@
pc().printf("PR: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
pc().printf("PR: heading cmd: %3.1f\r\n",headingLoop().getCommand());
- //show that this is the start of new rise sequence
- recordState(_state);
-
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -488,13 +448,12 @@
}
// what is active?
- pc().printf("PR: bce pos (cmd pos): %3.1f mm (%0.1f), batt pos: %3.1f mm (%0.1f), rudder: %f (depth: %3.1f ft, pitch: %3.1f, heading: %3.1f [cmd: %0.1f])[%0.2f sec] [imu heading: %0.1f] \r", bce().getPosition_mm(),bce().getSetPosition_mm(), batt().getPosition_mm(),batt().getSetPosition_mm(), rudder().getPosition_pwm(), depthLoop().getPosition(), pitchLoop().getPosition(), headingLoop().getPosition(), headingLoop().getOutput(), timer.read(), imu().getHeading());
+ pc().printf("PR: bce pos (cmd pos): %3.1f mm (%0.1f), batt pos: %3.1f mm (%0.1f), rudder: %0.1f (depth: %3.1f ft, pitch: %3.1f, heading: %3.1f [cmd: %0.1f])[%0.2f sec] [imu heading: %0.1f] \r", bce().getPosition_mm(),bce().getSetPosition_mm(), batt().getPosition_mm(),batt().getSetPosition_mm(), rudder().getPosition_pwm(), depthLoop().getPosition(), pitchLoop().getPosition(), headingLoop().getPosition(), headingLoop().getOutput(), timer.read(), imu().getHeading());
// ACTIVE RUDDER CONTROL
rudder().setPosition_deg(headingLoop().getOutput());
- //record data internally (for access by MbedLogger)
- recordData(_state);
+ checkMotorPositions();
break;
// NEW DIVE AND RISE SEQUENCES
@@ -518,12 +477,8 @@
//create the log file (works only if the file is closed)
//createNewFile();
- //show that this is the start of a new float level sequence
- recordState(_state);
-
//triggers logger array
_is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -544,9 +499,6 @@
pc().printf("FL: pitchLoop output: %3.1f, batt pos: %3.1f, piston pos: %3.1f [%0.1f sec]\r", pitchLoop().getOutput(), batt().getPosition_mm(), bce().getPosition_mm(), timer.read());
batt().setPosition_mm(pitchLoop().getOutput());
- //record data every 5 seconds
- recordData(_state);
-
break;
case FLOAT_BROADCAST :
@@ -568,12 +520,8 @@
//create the log file (works only if the file is closed)
//createNewFile();
- //show that this is the start of a new float broadcast sequence
- recordState(_state);
-
- //triggers logger arrayc
+ //triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -602,9 +550,6 @@
// what is active?
pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm, heading(IMU): %0.1f (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), imu().getHeading(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
- //record data every 5 seconds
- recordData(_state);
-
break;
case MULTI_DIVE :
@@ -628,17 +573,10 @@
pitchLoop().setCommand(sequence_pitch_command);
pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
- //create the log file (works only if the file is closed)
- //createNewFile();
-
- //show that this is the start of a new MULTI_DIVE sequence
- recordState(_state);
-
//no max depth recording right now
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -655,13 +593,12 @@
_isTimeoutRunning = false;
}
- // what is active?
+ // WHAT IS ACTIVE?
pc().printf("MULTI-DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
bce().setPosition_mm(depthLoop().getOutput());
batt().setPosition_mm(pitchLoop().getOutput());
- //record data every 5 seconds
- recordData(_state);
+ checkMotorPositions();
break;
@@ -692,12 +629,8 @@
//create the log file (works only if the file is closed)
//createNewFile();
- //show that this is the start of a new MULTI_DIVE sequence
- recordState(_state);
-
//triggers logger array
_is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- recordData(_state);
}
// how exit?
@@ -740,13 +673,12 @@
//have to stop this with the _multi_dive_counter variable!
}
- // what is active?
+ // WHAT IS ACTIVE?
pc().printf("MULTI-RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
batt().setPosition_mm(pitchLoop().getOutput());
- //record data every 5 seconds
- recordData(_state);
+ checkMotorPositions();
break;
@@ -846,7 +778,7 @@
// output the keyboard menu for user's reference
void StateMachine::showSimpleMenu() {
- pc().printf("\r\r\n\nSIMPLE KEYBOARD MENU (06/15/2018):\r\r\n"); //make sure depth sensor tares itself on startup
+ pc().printf("\r\r\n\nSIMPLE KEYBOARD MENU (06/22/2018):\r\r\n"); //make sure depth sensor tares itself on startup
pc().printf(" Neutral Positions BCE: %0.1f BMM: %0.1f \r\n", _neutral_bce_pos_mm, _neutral_batt_pos_mm);
pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
@@ -860,18 +792,21 @@
pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);
pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
-
+ pc().printf("A/S to decrease/increase depth setpoint: %3.1f (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
+
pc().printf("T to enter in the timeout (default is 60 seconds): %d s\r\n",_timeout);
pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
pc().printf(" 8 STREAM SENSOR STATUS (and channel readings)\r\n");
+ pc().printf(" '|' to tare the depth sensor (vertical bar)\r\n");
+
pc().printf(" ? to reset mbed\r\n");
pc().printf(" * (asterisk) to go to DEBUG keyboard menu\r\n");
}
void StateMachine::showDebugMenu() {
- pc().printf("\r\r\n\nDEBUG KEYBOARD MENU (06/15/2018):\r\r\n");
+ pc().printf("\r\r\n\nDEBUG KEYBOARD MENU (06/22/2018):\r\r\n");
pc().printf(" Y to go into CHECK NEUTRAL TUNING (This is on a timer! Uses NEUTRAL positions!)\r\n");
pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
pc().printf(" N to find neutral\r\n");
@@ -891,7 +826,7 @@
pc().printf("[/] to change bce neutral position: %0.1f\r\n", _neutral_bce_pos_mm);
pc().printf("</> to change batt neutral position: %0.1f\r\n", _neutral_batt_pos_mm);
pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitch_command);
- pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depth_command);
+ pc().printf("A/S to decrease/increase depth setpoint: %3.1f (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);
@@ -916,9 +851,9 @@
pc().printf(" * (asterisk) to go to SIMPLE keyboard menu\r\n");
}
-//Find Neutral sub finite state machine
-// Note: the sub-fsm only moves the pistons once at the start of each timer loop
-// (timer completes, move piston, timer completes, move piston, etc)
+//FIND_NEUTRAL Sub-Finite State Machine (sub-FSM)
+// Note: the sub-FSM only moves the pistons once at the start of each timer loop
+// (timer completes, moves piston, timer completes, moves piston, etc)
int StateMachine::runNeutralStateMachine() {
switch (_substate) {
case NEUTRAL_SINKING :
@@ -959,6 +894,8 @@
// what is active? (only the buoyancy engine moved every 5 seconds at start)
pc().printf("BCE current pos: %0.1f mm (BCE setpoint: %0.1f mm) (current depth: %0.1f ft)\r", bce().getPosition_mm(),bce().getSetPosition_mm(),depthLoop().getPosition()); //debug
+
+ checkMotorPositions(); //failsafe
break;
case NEUTRAL_SLOWLY_RISE:
@@ -1002,7 +939,9 @@
// what is active? (only the buoyancy engine moved every 5 seconds)
pc().printf("depthLoop getOutput: %0.1f\r", depthLoop().getOutput()); //debug
- bce().setPosition_mm(depthLoop().getOutput()); // (DID NOT WORK ON BENCH)
+ bce().setPosition_mm(depthLoop().getOutput());
+
+ checkMotorPositions(); //failsafe
break;
case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired
@@ -1069,6 +1008,8 @@
pc().printf("\r\n\n (BATT POS: %0.1f) moving 1 mm [timer: %0.1f]\r\n", batt().getPosition_mm(), timer.read());
_isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
}
+
+ checkMotorPositions(); //failsafe
break;
//this state could be removed, it is only used as a transition but is needed to stop entering this function
@@ -1333,10 +1274,6 @@
keyboard_menu_STREAM_STATUS(); //and channel readings for debugging
}
-// else if (user_input == '8') {
-// keyboard_menu_CHANNEL_READINGS();
-// }
-
else if (user_input == '9') {
_heading_command -= 5.0; //decrement the rudder setpoint
headingLoop().setCommand(_heading_command);
@@ -1513,6 +1450,17 @@
pc().printf("depth P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
pc().printf("pitch P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
}
+
+ else if (user_input == 'A' or user_input == 'a') {
+ _depth_command -= 0.5; //decrement the depth setpoint
+ depthLoop().setCommand(_depth_command);
+ pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
+ }
+ else if (user_input == 'S' or user_input == 's') {
+ _depth_command += 0.5; //increment the depth setpoint
+ depthLoop().setCommand(_depth_command);
+ pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
+ }
else if (user_input == '?') {
pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
@@ -1520,6 +1468,14 @@
mbed_reset();
}
+ else if (user_input == '|') {
+ pc().printf("taring depth sensor\r\n");
+ pc().printf("Pre-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
+ wait(0.1);
+ depth().tare(); // tares to ambient (do on surface)
+ pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
+ }
+
//POSITION DIVE COMMANDS
else if (user_input == 'k') {
_BCE_dive_offset -= 1.0;
@@ -1770,7 +1726,7 @@
// show the menu
pc().printf("\r\n7: MANUAL TUNING MENU (EXIT WITH 'X' !) (Pause and Unpause rudder ticker with P and U\n");
pc().printf("\r\n(Adjust BCE and BATT positions in real-time. Timeout NOT running! (decrease/increase BCE with A/S, BATT with Q/W, RUDDER with E/R)\r\n");
- pc().printf("\r\nMANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(), imu().getHeading());
+ pc().printf("\r\nMANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(), imu().getHeading());
//made these into internal parameters
float _tuning_bce_pos_mm = 300.0; //safe starting position
@@ -1793,7 +1749,7 @@
}
else {
- pc().printf("MANUAL_TUNING: (current stats) BCE pos: %3.1f mm (cmd: %3.1f mm), BATT pos: %3.1f mm (cmd: %3.1f mm) SERVO: %0.1f deg, %f pwm\r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm());
+ pc().printf("MT: POS (CMD) BCE %3.1f mm (%3.1f mm), BATT %3.1f mm (%3.1f mm) SERVO: %0.1f deg, (%0.1f pwm) PITCH: %0.1f,HEADING: %0.1f \r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm(),imu().getPitch(),imu().getHeading());
continue; // didn't get a user input, so keep waiting for it
}
@@ -2269,79 +2225,6 @@
}
}
-void StateMachine::recordData(int input_state) {
- string string_state;
- if (input_state == SIT_IDLE)
- string_state = "SIT_IDLE";
- else if (input_state == FIND_NEUTRAL)
- string_state = "FIND_NEUTRAL";
- else if (input_state == DIVE)
- string_state = "DIVE";
- else if (input_state == RISE)
- string_state = "RISE";
- else if (input_state == FLOAT_LEVEL)
- string_state = "FLOAT_LEVEL";
- else if (input_state == FLOAT_BROADCAST)
- string_state = "FLOAT_BROADCAST";
- else if (input_state == EMERGENCY_CLIMB)
- string_state = "EMERGENCY_CLIMB";
- else if (input_state == MULTI_DIVE)
- string_state = "MULTI_DIVE";
- else if (input_state == MULTI_RISE)
- string_state = "MULTI_RISE";
- else if (input_state == KEYBOARD)
- string_state = "KEYBOARD";
-
- if (!_is_log_timer_running) {
- //pc().printf("\r\n\nlog timer running...\r\n\n"); //debug
-
- _log_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds
- _is_log_timer_running = true; //disable this block after one iteration
-
- _data_log[0] = systemTime().read(); //system time reading
- _data_log[1] = depthLoop().getCommand(); //depth command
- _data_log[2] = depthLoop().getPosition(); //depth reading
- _data_log[3] = pitchLoop().getCommand(); //pitch command
- _data_log[4] = pitchLoop().getPosition(); //pitch reading
- _data_log[5] = bce().getSetPosition_mm();
- _data_log[6] = bce().getPosition_mm();
- _data_log[7] = batt().getSetPosition_mm();
- _data_log[8] = batt().getPosition_mm();
-
- //record data to the MBED every 5 seconds
- //mbedLogger().saveArrayToFile(string_state,input_state,_data_log);
- }
- if (timer.read() >= _log_timer) {
- _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
- }
-}
-
-void StateMachine::recordState(int input_state) {
- string string_state;
-
- if (input_state == SIT_IDLE)
- string_state = "SIT_IDLE";
- else if (input_state == FIND_NEUTRAL)
- string_state = "FIND_NEUTRAL";
- else if (input_state == DIVE)
- string_state = "DIVE";
- else if (input_state == RISE)
- string_state = "RISE";
- else if (input_state == FLOAT_LEVEL)
- string_state = "FLOAT_LEVEL";
- else if (input_state == FLOAT_BROADCAST)
- string_state = "FLOAT_BROADCAST";
- else if (input_state == EMERGENCY_CLIMB)
- string_state = "EMERGENCY_CLIMB";
- else if (input_state == MULTI_DIVE)
- string_state = "MULTI_DIVE";
- else if (input_state == MULTI_RISE)
- string_state = "MULTI_RISE";
- else if (input_state == KEYBOARD)
- string_state = "KEYBOARD";
- //datalogger().printf("%s\n", string_state.c_str());
-}
-
float * StateMachine::dataArray() {
//return the array to a calling function
return _data_log;
@@ -2385,4 +2268,16 @@
}
return float_conversion;
+}
+
+//added because channel readings failed multiple times (if they fail, motors could break parts)
+void StateMachine::checkMotorPositions() {
+
+ /* 1. If channel readings fail, calculations will give negative positions
+ 2. If the potentiometers are reporting negative positions, send the system into float broadcast
+ 3. The motors will move in the correct direction and if something is wrong the will go to their full extension but be at the surface */
+
+ if (bce().getPosition_mm() < 0 or batt().getPosition_mm() < 0 ) {
+ //_state = FLOAT_BROADCAST;
+ }
}
\ No newline at end of file