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:
- 32:f2f8ae34aadc
- Parent:
- 30:2964617e7676
- Child:
- 33:29a4268fbc74
- Child:
- 34:9b66c5188051
--- a/StateMachine/StateMachine.cpp Wed Dec 06 22:45:23 2017 +0000
+++ b/StateMachine/StateMachine.cpp Wed Dec 20 13:52:50 2017 +0000
@@ -2,15 +2,15 @@
#include "StaticDefs.hpp"
StateMachine::StateMachine() {
- _timeout = 480; // generic timeout for every state, seconds
+ _timeout = 20; // generic timeout for every state, seconds
_pitchTolerance = 5.0; // pitch angle tolerance for FLOAT_LEVEL state
_bceFloatPosition = bce().getTravelLimit(); // bce position for "float" states (max travel limit for BCE is 320 mm)
_battFloatPosition = batt().getTravelLimit(); // batt position tail high for "broadcast" state (max travel limit for battery is 75 mm)
- _depthCommand = 2.0; // user keyboard depth (default)
- _pitchCommand = -20.0; // user keyboard pitch (default)
+ _depth_command = 2.0; // user keyboard depth (default)
+ _pitch_command = -20.0; // user keyboard pitch (default)
_neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM
@@ -44,6 +44,14 @@
_max_recorded_depth_neutral = -99; //float to record max depth
_max_recorded_depth_dive = -99; //float to record max depth
+
+ _neutral_sink_command_mm = -2.5; //defaults for neutral finding sub-FSM
+ _neutral_rise_command_mm = 2.0;
+ _neutral_pitch_command_mm = 0.5;
+
+ _max_recorded_auto_neutral_depth = -99;
+
+ _file_closed = true;
}
//Finite State Machine (FSM)
@@ -61,9 +69,12 @@
// what is active?
bce().pause();
batt().pause();
-
+
//reset sub FSM
_isSubStateTimerRunning = false;
+
+ //close the MBED file
+ _file_closed = true;
}
// how exit?
@@ -85,6 +96,16 @@
// 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
+ mbedLogger().saveSequenceStringToFile("EMERGENCY_CLIMB");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
}
// how exit?
@@ -100,6 +121,10 @@
timer.reset();
_isTimeoutRunning = false;
}
+
+ //record data every 5 seconds
+ recordData();
+
break;
case FIND_NEUTRAL :
@@ -125,7 +150,17 @@
_substate_array[_substate_array_counter] = NEUTRAL_SINKING; //save to state array
_substate_array_counter++;
- runNeutralStateMachine();
+ 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
+ mbedLogger().saveSequenceStringToFile("FIND_NEUTRAL");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
}
// how exit? (exit with the timer, if timer still running continue processing sub FSM)
@@ -148,10 +183,15 @@
_state = RISE;
_isTimeoutRunning = false;
}
+
+ //record data every 5 seconds
+ recordData();
+
break;
case DIVE :
// start local state timer and init any other one-shot actions
+
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: DIVE\r\n");
timer.reset(); // timer goes back to zero
@@ -163,24 +203,39 @@
batt().unpause();
// what are the commands?
- depthLoop().setCommand(_depthCommand);
- pitchLoop().setCommand(_pitchCommand);
- pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
- pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
+ depthLoop().setCommand(_depth_command);
+ pitchLoop().setCommand(_pitch_command);
+
+ //variables to record every 1-4 seconds
+ _depth_command = depthLoop().getCommand();
+ _pitch_command = pitchLoop().getCommand();
+
+ pc().printf("DIVE: depth cmd: %3.1f\r\n",_depth_command);
+ pc().printf("DIVE: pitch cmd: %3.1f\r\n",_pitch_command);
//reset max dive depth
_max_recorded_depth_dive = -99; //float to record max depth
+
+ //create the log file (works only if the file is closed)
+ createNewFile();
+
+ //show that this is the start of new dive sequence
+ mbedLogger().saveSequenceStringToFile("DIVE");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
}
// how exit?
- if (timer > _timeout) {
+ if (timer.read() > _timeout) {
pc().printf("DIVE: timed out\n\n\r");
_state = RISE; //new behavior 11/17/2017
timer.reset();
_isTimeoutRunning = false;
}
- else if (depthLoop().getPosition() > depthLoop().getCommand()) {
- pc().printf("DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+ else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches
+ pc().printf("DIVE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
_state = RISE;
timer.reset();
_isTimeoutRunning = false;
@@ -188,16 +243,21 @@
// 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]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
- bce().setPosition_mm(depthLoop().getOutput());
+ bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
batt().setPosition_mm(pitchLoop().getOutput());
if (depthLoop().getPosition() > _max_recorded_depth_dive) { //debug
_max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded
}
+
+ //record data every 5 seconds
+ recordData();
+
break;
case RISE :
// start local state timer and init any other one-shot actions
+
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: RISE\r\n");
timer.reset(); // timer goes back to zero
@@ -210,20 +270,38 @@
// what are the commands?
depthLoop().setCommand(-1.0); //make sure to get towards the surface (saw issues at LASR pool)
- pitchLoop().setCommand(-_pitchCommand);
- pc().printf("RISE: depth cmd: 0.0\r\n");
- pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
+ pitchLoop().setCommand(-_pitch_command);
+
+ //variables to record every 1-4 seconds
+ _depth_command = depthLoop().getCommand();
+ _pitch_command = pitchLoop().getCommand();
+
+ pc().printf("RISE: depth cmd: %0.1f\r\n", _depth_command);
+ pc().printf("RISE: pitch cmd: %0.1f\r\n",_pitch_command);
+
+ //create the log file (works only if the file is closed)
+ createNewFile();
+
+ //show that this is the start of new rise sequence
+ mbedLogger().saveSequenceStringToFile("RISE");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
}
// how exit?
- if (timer > _timeout) {
+ if (timer.read() > _timeout) {
pc().printf("RISE: timed out\r\n");
_state = EMERGENCY_CLIMB;
timer.reset();
_isTimeoutRunning = false;
}
- else if (depthLoop().getPosition() < 0.5) { //removed depthLoop().getCommand()
- pc().printf("RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+
+ //modified from (depthLoop().getPosition() < depthLoop().getCommand() + 0.5)
+ //did not work correctly in bench test (stuck in rise state)
+ else if (depthLoop().getPosition() < 0.5) {
+ pc().printf("RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
_state = FLOAT_BROADCAST;
timer.reset();
_isTimeoutRunning = false;
@@ -232,7 +310,11 @@
// what is active?
pc().printf("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());
+ batt().setPosition_mm(pitchLoop().getOutput());
+
+ //record data every 5 seconds
+ recordData();
+
break;
case FLOAT_LEVEL :
@@ -250,6 +332,16 @@
// what are the commands?
bce().setPosition_mm(_bceFloatPosition);
pitchLoop().setCommand(0.0);
+
+ //create the log file (works only if the file is closed)
+ createNewFile();
+
+ //show that this is the start of a new float level sequence
+ mbedLogger().saveSequenceStringToFile("FLOAT_LEVEL");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
}
// how exit?
@@ -269,6 +361,10 @@
// what is active?
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();
+
break;
case FLOAT_BROADCAST :
@@ -286,6 +382,16 @@
// what are the commands?
bce().setPosition_mm(_bceFloatPosition);
batt().setPosition_mm(_battFloatPosition);
+
+ //create the log file (works only if the file is closed)
+ createNewFile();
+
+ //show that this is the start of a new float broadcast sequence
+ mbedLogger().saveSequenceStringToFile("BROADCAST");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
}
// how exit?
@@ -293,6 +399,10 @@
pc().printf("FB: timed out\r\n");
_state = SIT_IDLE;
timer.reset();
+
+ //stop recording data
+ mbedLogger().closeFile();
+
_isTimeoutRunning = false;
}
else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
@@ -300,11 +410,19 @@
pc().printf("FB: position: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(), bce().getDeadband());
_state = SIT_IDLE;
timer.reset();
+
+ //stop recording data
+ mbedLogger().closeFile();
+
_isTimeoutRunning = false;
}
// what is active?
- pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (setPos batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
+ pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
+
+ //record data every 5 seconds
+ recordData();
+
break;
case MULTI_DIVE :
@@ -320,13 +438,25 @@
batt().unpause();
//retrieve commands from structs (loaded from sequence.cfg file)
- float sequence_depthCommand = currentStateStruct.depth;
- float sequence_pitchCommand = currentStateStruct.pitch;
+ float sequence_depth_command = currentStateStruct.depth;
+ float sequence_pitch_command = currentStateStruct.pitch;
// what are the commands?
- depthLoop().setCommand(sequence_depthCommand);
- pitchLoop().setCommand(sequence_pitchCommand);
+ depthLoop().setCommand(sequence_depth_command);
+ 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
+ mbedLogger().saveSequenceStringToFile("MULTI_DIVE");
+ recordState(_state);
+
+ //no max depth recording right now
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
}
// how exit?
@@ -347,6 +477,10 @@
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();
+
break;
case MULTI_RISE :
@@ -366,12 +500,22 @@
stateMachine().getDiveSequence();
//retrieve just pitch command from struct
- float sequence_pitchCommand = currentStateStruct.pitch;
+ float sequence_pitch_command = currentStateStruct.pitch;
// what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017
depthLoop().setCommand(0.5);
- pitchLoop().setCommand(-sequence_pitchCommand);
+ pitchLoop().setCommand(-sequence_pitch_command);
pc().printf("MULTI-RISE: depth cmd: 0.0 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
+ mbedLogger().saveSequenceStringToFile("MULTI_RISE");
+ recordState(_state);
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
}
// how exit?
@@ -413,7 +557,126 @@
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();
+
break;
+
+ case FIND_AUTO_NEUTRAL_DEPTH :
+ //RUN TO DEPTH, if this doesn't work, surface; if it does, continue to FIND_AUTO_NEUTRAL_PITCH
+
+ // start local state timer and init any other one-shot actions
+ if (!_isTimeoutRunning) {
+ pc().printf("\r\n\nstate: FIND_AUTO_NEUTRAL_DEPTH\r\n");
+ timer.reset(); // timer goes back to zero
+ timer.start(); // background timer starts running
+ _isTimeoutRunning = true;
+
+ // what needs to be started?
+ bce().unpause();
+ //BATTERY NOT MOVING
+
+ // what are the commands?
+ depthLoop().setCommand(_depth_command);
+ pc().printf("FIND_AUTO_NEUTRAL_DEPTH: depth cmd: %3.1f\r\n",depthLoop().getCommand());
+
+ //reset max dive depth
+ _max_recorded_auto_neutral_depth = -99; //float to record max depth
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+ }
+
+ // how exit?
+ if (timer > _timeout) {
+ pc().printf("FIND_AUTO_NEUTRAL_DEPTH: timed out (next state: EMERGENCY_CLIMB)\n\n\r");
+ _state = EMERGENCY_CLIMB;
+ _isTimeoutRunning = false;
+ }
+
+ //transition only when the PV settles and is at depth (then save)
+ else if (depthLoop().getPosition() > depthLoop().getCommand()) {
+ if (depthLoop().getVelocity() < 1.0) { //less than 1 foot/s
+ //if successful, save the data
+ _neutral_bce_pos_mm = bce().getPosition_mm(); //get the position of the battery motor
+
+ pc().printf("FIND_AUTO_NEUTRAL_DEPTH: Saving neutral (depth) Buoyancy Engine position: %0.1f mm\n\n\r", _neutral_bce_pos_mm);
+ configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
+
+ //transition to the next state
+ _state = FIND_AUTO_NEUTRAL_PITCH;
+ _isTimeoutRunning = false;
+ }
+ }
+
+ // what is active?
+ pc().printf("FIND_AUTO_NEUTRAL_DEPTH: bce pos: %0.1f mm, batt pos: %0.1f mm (depth: %0.1f ft) (pitch: %0.1f deg)[%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
+ bce().setPosition_mm(depthLoop().getOutput());
+ //BATTERY NOT ACTIVE
+
+ //record max depth for testing
+ if (depthLoop().getPosition() > _max_recorded_auto_neutral_depth) { //debug
+ _max_recorded_auto_neutral_depth = depthLoop().getPosition(); //new max depth recorded
+ }
+
+ //record data every 5 seconds
+ recordData();
+
+ break;
+
+ case FIND_AUTO_NEUTRAL_PITCH :
+ //RUN TO DEPTH, if this doesn't work, surface; if it does, continue to FIND_AUTO_NEUTRAL_PITCH
+
+ // start local state timer and init any other one-shot actions
+ if (!_isTimeoutRunning) {
+ pc().printf("\r\n\nstate: FIND_AUTO_NEUTRAL_PITCH\r\n");
+ timer.reset(); // timer goes back to zero
+ timer.start(); // background timer starts running
+ _isTimeoutRunning = true;
+
+ // what needs to be started?
+ batt().unpause();
+ bce().pause(); //BUOYANCY ENGINE NOT MOVING
+
+
+ // what are the commands?
+ pitchLoop().setCommand(0.0);
+ pc().printf("FIND_AUTO_NEUTRAL_PITCH: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
+
+ //this was missing
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+ }
+
+ // how exit?
+ if (timer > _timeout) {
+ pc().printf("FIND_AUTO_NEUTRAL_PITCH: timed out (next state: EMERGENCY_CLIMB)\n\n\r");
+ _state = EMERGENCY_CLIMB;
+ _isTimeoutRunning = false;
+ }
+
+ //transition only when the PV settles and is at depth (within 2 degrees and rate is less than 2 deg/s) (then save)
+ else if ((fabs(pitchLoop().getPosition()) < 2.0) && (pitchLoop().getVelocity() < 2.0)) {
+ //if successful, save the data
+ _neutral_batt_pos_mm = batt().getPosition_mm(); //get the position of the battery motor
+
+ pc().printf("FIND_AUTO_NEUTRAL_PITCH: Saving neutral (pitch) battery position: %0.1f mm\n\n\r", _neutral_batt_pos_mm);
+ configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
+
+ //transition to the next state
+ _state = RISE;
+ _isTimeoutRunning = false;
+ }
+
+ // what is active?
+ pc().printf("FIND_AUTO_NEUTRAL_PITCH: bce pos: %0.1f mm, batt pos: %0.1f mm (depth: %0.1f ft) (pitch: %0.1f deg)[%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
+ batt().setPosition_mm(pitchLoop().getOutput()); //trying to get to zero pitch
+ //BUOYANCY ENGINE NOT ACTIVE
+
+ //record data every 5 seconds
+ recordData();
+
+ break;
default :
pc().printf("DEBUG: SIT_IDLE\n\r");
@@ -432,6 +695,8 @@
// output the keyboard menu for user's reference
void StateMachine::showMenu() {
pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
+ pc().printf(" G to initiate auto-neutral sequence\r\n");
+ pc().printf(" F to jump to auto-neutral FIND PITCH state\r\n");
pc().printf(" N to find neutral\r\n");
pc().printf(" M to initiate multi-dive cycle\r\n");
pc().printf(" D to initiate dive cycle\r\n");
@@ -442,10 +707,12 @@
//pc().printf(" H to run homing sequence on both BCE and Batt\r\n");
pc().printf(" T to tare the depth sensor\r\n");
pc().printf(" Z to show FSM and sub-FSM states.\r\n");
- pc().printf("[/] to change bce neutral position\r\n");
- pc().printf("</> to change batt neutral position\r\n");
- pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitchCommand);
- pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depthCommand);
+ pc().printf(" P to print the current log file.\r\n");
+ pc().printf(" X to print the list of log files.\r\n");
+ 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("+/- to decrease/increase timeout: %d s\r\n",_timeout);
pc().printf(" 1 BCE PID sub-menu\r\n");
pc().printf(" 2 BATT PID sub-menu\r\n");
@@ -467,14 +734,11 @@
pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition());
- // what are the commands?
- //move piston at start of sequence (retract 2.5 mm)
- bce().setPosition_mm(bce().getSetPosition_mm() - 2.5); //no depth command
+ // what are the commands? (BCE linear actuator active, no pitch movement)
+ //move piston at start of sequence (default: retract 2.5 mm)
+ bce().setPosition_mm(bce().getSetPosition_mm() - _neutral_sink_command_mm); //no depth command
- // it's okay to run the pitch outer loop now since we've already found pitch level in the previous state
- pitchLoop().setCommand(0.0);
-
- pc().printf("NEUTRAL_SINKING: Retracting piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition());
+ pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", _neutral_sink_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition());
_isSubStateTimerRunning = true; //disable this block after one iteration
}
@@ -487,7 +751,7 @@
_isSubStateTimerRunning = false; // reset the sub state timer
}
//once deeper than the commanded setpoint...
- else if (depthLoop().getPosition() > _depthCommand) {
+ else if (depthLoop().getPosition() > _depth_command) {
_substate = NEUTRAL_SLOWLY_RISE; // next state
_isSubStateTimerRunning = false; //reset the sub state timer
}
@@ -495,13 +759,13 @@
// what is active?
//once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint
if (timer.read() >= _neutral_timer) {
- pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! Retracting BCE piston 5 mm [current time: %0.1f]\r\n", timer.read());
+ pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! [current time: %0.1f]\r\n", timer.read());
_isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
}
-
- // what is active?
- batt().setPosition_mm(pitchLoop().getOutput()); // pitch outer loop is running
+ // what is active? (only the buoyancy engine moved every 5 seconds)
+ pc().printf("depthLoop getOutput (position): %0.1f (current depth: %0.1f ft)\r", depthLoop().getOutput(), depthLoop().getPosition()); //debug
+ bce().setPosition_mm(depthLoop().getOutput()); // (DID NOT WORK ON BENCH)
break;
case NEUTRAL_SLOWLY_RISE:
@@ -511,13 +775,13 @@
pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Next extension at %0.1f sec) [current time: %0.1f]\r\n",_neutral_timer,timer.read());
// what are the commands?
- //move piston at start of sequence (extend)
- bce().setPosition_mm(bce().getSetPosition_mm() + 2); //no depth command
+ //move piston at start of sequence (default: extend 2.0 mm)
+ bce().setPosition_mm(bce().getSetPosition_mm() + _neutral_rise_command_mm); //no depth command
// it's okay to run the pitch outer loop now since we've already found pitch level in the previous state
pitchLoop().setCommand(0.0);
- pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand());
+ pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", _neutral_rise_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand());
_isSubStateTimerRunning = true; //disable this block after one iteration
}
@@ -538,31 +802,31 @@
// what is active?
//once 5 second timer complete, reset the timeout so the state one-shot entry will move the setpoint
if (timer.read() >= _neutral_timer) {
- pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! Extending 1 mm [timer: %0.1f]\r\n", timer.read());
+ pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! [timer: %0.1f]\r\n", timer.read());
_isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
}
- // what is active?
- batt().setPosition_mm(pitchLoop().getOutput()); // pitch outer loop is running
+ // 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)
break;
case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired
- case NEUTRAL_FIRST_PITCH :
// start local state timer and init any other one-shot actions
if (!_isSubStateTimerRunning) {
_neutral_timer = timer.read() + 10; // record time when this block is entered and add several seconds
pc().printf("\r\nNEUTRAL_CHECK_PITCH: Next move in %0.1f sec \r\n",_neutral_timer - timer.read());
- // what are the commands?
+ // what are the commands? (default: retract or extend 0.5 mm)
if (pitchLoop().getPosition() > 2) { // nose is high
- batt().setPosition_mm(batt().getSetPosition_mm() + 0.5); // move battery forward (using setpoint from linear actuator)
- pc().printf("\n\rNeutral Check Pitch: moving battery FWD in 1mm increments\n\n\r");
+ batt().setPosition_mm(batt().getSetPosition_mm() + _neutral_pitch_command_mm); // move battery forward (using setpoint from linear actuator)
+ pc().printf("\n\rNeutral Check Pitch: moving battery FWD in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
}
else if (pitchLoop().getPosition() < -2) { // nose is low
- batt().setPosition_mm(batt().getSetPosition_mm() - 0.5); // move battery aft (using setpoint from linear actuator)
- pc().printf("\n\rNeutral Check Pitch: moving battery AFT in 1mm increments\n\n\r");
+ batt().setPosition_mm(batt().getSetPosition_mm() - _neutral_pitch_command_mm); // move battery aft (using setpoint from linear actuator)
+ pc().printf("\n\rNeutral Check Pitch: moving battery AFT in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
}
_isSubStateTimerRunning = true; //disable this block after one iteration
@@ -579,18 +843,8 @@
_max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded
}
- if (_substate == NEUTRAL_FIRST_PITCH) {
- _substate = NEUTRAL_SINKING; // next state starts the sinking
- _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
-
- // save this neutral (not in file) for pitch
- pitchLoop().setOutputOffset(batt().getPosition_mm());
-
- pc().printf("substate: NEUTRAL_FIRST_PITCH (next substate: NEUTRAL_SINKING)\n\r");
- }
-
// found level and at depth too, so save it all now
- else if (_substate == NEUTRAL_CHECK_PITCH) {
+ if (_substate == NEUTRAL_CHECK_PITCH) {
//save positions locally
_neutral_batt_pos_mm = batt().getPosition_mm();
_neutral_bce_pos_mm = bce().getPosition_mm();
@@ -603,7 +857,7 @@
configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset
configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset
- pc().printf("\n\rSaving Positions: BCE: %0.1f mm, BATT: %0.1f\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
+ pc().printf("\n\n\r>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
_substate = NEUTRAL_EXIT;
_isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
@@ -667,6 +921,160 @@
return _substate; // message to calling function of what sub-state it's in
}
}
+
+////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)
+//int StateMachine::runActiveNeutralStateMachine() {
+// switch (_substate) {
+// case NEUTRAL_AUTO_DEPTH :
+// //start the 10 second timer
+// if (!_isSubStateTimerRunning) {
+// _neutral_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds
+//
+// pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition());
+//
+// // what are the commands? (BCE linear actuator active, no pitch movement)
+// //move piston at start of sequence (default: retract 2.5 mm)
+// bce().setPosition_mm(bce().getSetPosition_mm() - _neutral_sink_command_mm); //no depth command
+//
+// pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", _neutral_sink_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition());
+//
+// _isSubStateTimerRunning = true; //disable this block after one iteration
+// }
+//
+// // how exit?
+// //once reached the travel limit, no need to keep trying, so exit
+// if (bce().getPosition_mm() <= 0) {
+// pc().printf("\n\rDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\n\r", bce().getPosition_mm());
+// _substate = NEUTRAL_EXIT;
+// _isSubStateTimerRunning = false; // reset the sub state timer
+// }
+// //once deeper than the commanded setpoint...
+// else if (depthLoop().getPosition() > _depth_command) {
+// _substate = NEUTRAL_SLOWLY_RISE; // next state
+// _isSubStateTimerRunning = false; //reset the sub state timer
+// }
+//
+// // what is active?
+// //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint
+// if (timer.read() >= _neutral_timer) {
+// pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! [current time: %0.1f]\r\n", timer.read());
+//
+// _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
+// }
+// // what is active? (only the buoyancy engine moved every 5 seconds)
+// break;
+//
+// case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired
+// // start local state timer and init any other one-shot actions
+//
+// if (!_isSubStateTimerRunning) {
+// _neutral_timer = timer.read() + 10; // record time when this block is entered and add several seconds
+// pc().printf("\r\nNEUTRAL_CHECK_PITCH: Next move in %0.1f sec \r\n",_neutral_timer - timer.read());
+//
+// // what are the commands? (default: retract or extend 0.5 mm)
+// if (pitchLoop().getPosition() > 2) { // nose is high
+// batt().setPosition_mm(batt().getSetPosition_mm() + _neutral_pitch_command_mm); // move battery forward (using setpoint from linear actuator)
+// pc().printf("\n\rNeutral Check Pitch: moving battery FWD in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
+// }
+// else if (pitchLoop().getPosition() < -2) { // nose is low
+// batt().setPosition_mm(batt().getSetPosition_mm() - _neutral_pitch_command_mm); // move battery aft (using setpoint from linear actuator)
+// pc().printf("\n\rNeutral Check Pitch: moving battery AFT in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
+// }
+//
+// _isSubStateTimerRunning = true; //disable this block after one iteration
+// }
+//
+// // how exit?
+// //pitch angle and pitch rate within small tolerance
+// //benchtop tests confirm angle needs to be around 2 degrees
+// if ((fabs(pitchLoop().getPosition()) < 2.0) and (fabs(pitchLoop().getVelocity()) < 5.0)) {
+// pc().printf("Debug: Found Level (NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH)\n\r"); //debug
+// // found level, but don't need to save anything this time
+//
+// if (depthLoop().getPosition() > _max_recorded_depth_neutral) { //debug
+// _max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded
+// }
+//
+// // found level and at depth too, so save it all now
+// if (_substate == NEUTRAL_CHECK_PITCH) {
+// //save positions locally
+// _neutral_batt_pos_mm = batt().getPosition_mm();
+// _neutral_bce_pos_mm = bce().getPosition_mm();
+//
+// //set the neutral positions in each outer loop
+// depthLoop().setOutputOffset(_neutral_bce_pos_mm);
+// pitchLoop().setOutputOffset(_neutral_batt_pos_mm);
+//
+// // save into the depth.txt and pitch.txt files
+// configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset
+// configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset
+//
+// pc().printf("\n\rSaving Positions: BCE: %0.1f mm, BATT: %0.1f\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
+//
+// _substate = NEUTRAL_EXIT;
+// _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
+// }
+//
+// else {
+// pc().printf("\n\rDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\n\r");
+// _substate = NEUTRAL_EXIT;
+// }
+// }
+//
+// // what is active?
+// //once timer complete, reset the timeout so the state one-shot entry will move the setpoint
+// if (timer.read() >= _neutral_timer) {
+// pc().printf("\r\n\nlevel timer COMPLETE!");
+// 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
+// }
+// break;
+//
+// //this state could be removed, it is only used as a transition but is needed to stop entering this function
+// case NEUTRAL_EXIT :
+// pc().printf("substate: NEUTRAL_EXIT\n\r");
+// break;
+//
+// default :
+// pc().printf("how did we get to substate: default?\n\r"); //debug
+// //a default within the sub-state machine
+// _substate = NEUTRAL_EXIT;
+// break;
+// }
+//
+// // reset the sub-FSM if needed (useful if you need to redo the neutral-finding sequence)
+// if (_substate == NEUTRAL_EXIT) {
+// pc().printf("******************************** EXITING sub-FSM! *******************************\n\n\r");
+//
+// //reset internal sub-state back to first entry conditions (first state is immediately sinking)
+// _substate = NEUTRAL_SINKING;
+// _isSubStateTimerRunning = false; // reset the sub state timer
+//
+// //record sub-states to view after sequence
+// _substate_array[_substate_array_counter] = NEUTRAL_EXIT; //save exit to state array
+// _substate_array_counter++;
+//
+// //reset _previous_substate on exit (has to be done in FIND_NEUTRAL if emergency exit)
+// _previous_substate = -1;
+//
+// //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed
+// return NEUTRAL_EXIT; // message to calling function we just exited
+// }
+// else {
+// //record sub-states to view after sequence (when changed)
+// if (_previous_substate != _substate) {
+// _substate_array[_substate_array_counter] = _substate; //save current state to state array
+// _substate_array_counter++;
+//
+// //record the current substate for comparison
+// _previous_substate = _substate;
+// }
+//
+// return _substate; // message to calling function of what sub-state it's in
+// }
+//}
// keyboard runs independently of the state machine, handling one key at a time
//keyboard updates the desired _keyboard_state that is used in the state machine
@@ -694,6 +1102,12 @@
if (userInput == 'D' or userInput == 'd') {
_keyboard_state = DIVE;
}
+ else if (userInput == 'G') {
+ _keyboard_state = FIND_AUTO_NEUTRAL_DEPTH; //new test 12/7/2017
+ }
+ else if (userInput == 'F') {
+ _keyboard_state = FIND_AUTO_NEUTRAL_PITCH; //new test 12/7/2017
+ }
else if (userInput == 'N' or userInput == 'n') {
_keyboard_state = FIND_NEUTRAL;
}
@@ -720,6 +1134,19 @@
else if (userInput == 'E' or userInput == 'e') {
_keyboard_state = EMERGENCY_CLIMB;
}
+ else if (userInput == 'P') {
+ //Print current SD card log file
+ printCurrentSdLog();
+ mbedLogger().printFromLogFile(); //print the current log file to the screen
+ }
+ else if (userInput == 'X') {
+ printDirectory();
+ //mbedLogger().printDirectory(); //print all log files to the screen
+ }
+ else if (userInput == 'C') {
+ //Transmit data (work in progress)
+ transmitData();
+ }
// else if (userInput == 'H' or userInput == 'h') {
// pc().printf("running homing procedure\r\n");
// bce().unpause(); bce().homePiston(); bce().pause();
@@ -757,9 +1184,7 @@
string string_substate;
for (int i = 0; i < _substate_array_counter; i++) {
- if (_substate_array[i] == NEUTRAL_FIRST_PITCH)
- string_substate = "NEUTRAL_FIRST_PITCH";
- else if (_substate_array[i] == NEUTRAL_SINKING)
+ if (_substate_array[i] == NEUTRAL_SINKING)
string_substate = "NEUTRAL_SINKING";
else if (_substate_array[i] == NEUTRAL_SLOWLY_RISE)
string_substate = "NEUTRAL_SLOWLY_RISE";
@@ -782,20 +1207,32 @@
}
else if (userInput == '[' or userInput == '{') {
- depthLoop().setOutputOffset(depthLoop().getOutputOffset() - 1); // decrease the bce neutral setpoint
- pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset());
+ _neutral_bce_pos_mm = depthLoop().getOutputOffset() - 1;
+ depthLoop().setOutputOffset(_neutral_bce_pos_mm); // decrease the bce neutral setpoint
+ pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
+ // save neutral depth value to config file
+ configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
}
else if (userInput == ']' or userInput == '}') {
- depthLoop().setOutputOffset(depthLoop().getOutputOffset() + 1); // increase the bce neutral setpoint
- pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset());
+ _neutral_bce_pos_mm = depthLoop().getOutputOffset() + 1;
+ depthLoop().setOutputOffset(_neutral_bce_pos_mm); // increase the bce neutral setpoint
+ pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
+ // save neutral depth value to config file
+ configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
}
else if (userInput == '<' or userInput == ',') {
- pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() - 1); // decrease the batt neutral setpoint
- pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset());
+ _neutral_batt_pos_mm = pitchLoop().getOutputOffset() - 1;
+ pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // decrease the batt neutral setpoint
+ pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
+ // save neutral pitch value to config file
+ configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
}
else if (userInput == '>' or userInput == '.') {
- pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() + 1); // increase the batt neutral setpoint
- pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset());
+ _neutral_batt_pos_mm = pitchLoop().getOutputOffset() + 1;
+ pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // increase the batt neutral setpoint
+ pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
+ // save neutral pitch value to config file
+ configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
}
else if (userInput == '?') {
@@ -806,23 +1243,23 @@
// change settings
else if (userInput == 'Q' or userInput == 'q') {
- _pitchCommand -= 0.5; //decrement the pitch setpoint
- pitchLoop().setCommand(_pitchCommand);
+ _pitch_command -= 0.5; //decrement the pitch setpoint
+ pitchLoop().setCommand(_pitch_command);
pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
}
else if (userInput == 'W' or userInput == 'w') {
- _pitchCommand += 0.5; //increment the pitch setpoint
- pitchLoop().setCommand(_pitchCommand);
+ _pitch_command += 0.5; //increment the pitch setpoint
+ pitchLoop().setCommand(_pitch_command);
pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
}
else if (userInput == 'A' or userInput == 'a') {
- _depthCommand -= 0.5; //decrement the depth setpoint
- depthLoop().setCommand(_depthCommand);
+ _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 (userInput == 'S' or userInput == 's') {
- _depthCommand += 0.5; //increment the depth setpoint
- depthLoop().setCommand(_depthCommand);
+ _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 (userInput == '-') {
@@ -849,18 +1286,21 @@
}
else if (userInput == 'C' or userInput == 'c') {
- pc().printf("depth: %3.1f\r\n",depthLoop().getPosition());
- pc().printf("pitch: %3.1f\r\n",imu().getPitch());
+
+ pc().printf("\n\n\rCURRENT STATUS AND PARAMETERS:\n\r");
+ pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
+ pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
- pc().printf("Latest Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\r\n",_neutral_batt_pos_mm,_neutral_bce_pos_mm);
+
+ pc().printf("\n\rNeutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());
- pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive);
+ pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
}
//when you read the keyboard successfully, change the state
@@ -1118,7 +1558,7 @@
pitchLoop().setControllerI(_pitch_KI);
pitchLoop().setControllerD(_pitch_KD);
- // save pitch PID values for outer loop
+ // save pitch PID values for outer loop (must save neutral position also)
configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
break; //exit the while loop
}
@@ -1132,11 +1572,23 @@
}
float StateMachine::getDepthCommand() {
- return _depthCommand;
+ return _depth_command;
}
float StateMachine::getPitchCommand() {
- return _pitchCommand;
+ return _pitch_command;
+}
+
+float StateMachine::getDepthReading() {
+ return _depth_reading;
+}
+
+float StateMachine::getPitchReading() {
+ return _pitch_reading;
+}
+
+float StateMachine::getTimerReading() {
+ return _timer_reading;
}
void StateMachine::setState(int input_state) {
@@ -1152,11 +1604,11 @@
}
void StateMachine::setDepthCommand(float input_depth_command) {
- _depthCommand = input_depth_command;
+ _depth_command = input_depth_command;
}
void StateMachine::setPitchCommand(float input_pitch_command) {
- _pitchCommand = input_pitch_command;
+ _pitch_command = input_pitch_command;
}
void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) {
@@ -1179,4 +1631,119 @@
currentStateStruct.pitch = sequenceController().sequenceStructLoaded[_multi_dive_counter].pitch;
_timeout = currentStateStruct.timeout; //set timeout before exiting this function
+}
+
+void StateMachine::recordState(int input_state) {
+ string string_state;
+
+ for (int i = 0; i < _state_array_counter; i++) {
+ 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());
+}
+
+void StateMachine::recordData() {
+ if (!_is_log_timer_running) {
+ //pc().printf("\n\n\rlog timer running...\n\n\r"); //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] = timer.read(); //timer reading (shouldn't this be constant throughout the state?)
+ _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(_data_log);
+
+ //record data to OpenLog every 5 seconds
+ datalogger().printf("%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f\n",_data_log[0],_data_log[1],
+ _data_log[2],_data_log[3],_data_log[4],_data_log[5],_data_log[6],_data_log[7],_data_log[8]);
+ }
+ if (timer.read() >= _log_timer) {
+ _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+ //pc().printf("\n\n\r TIME READ >= _log_timer \n\n\r"); //debug
+ }
+}
+
+void StateMachine::printDirectory() {
+ //create a DirectoryList object that points to the local directory
+ DirectoryList mbed_dir( "/local" );
+
+ if ( mbed_dir.error_check() ) {
+ //error( "MBED directory could not be opened\r\n" );
+ pc().printf("MBED directory could not be opened\r\n");
+ }
+
+ pc().printf("\n\rFiles in MBED directory:\n\r");
+ for ( int i = 0; i < mbed_dir.size(); i++ )
+ pc().printf( "%s\r\n", mbed_dir[ i ].c_str() );
+
+ //SD CARD DIRECTORY
+ DirectoryList sd_dir( "/sd" );
+
+ if ( sd_dir.error_check() ) {
+ //error( "MBED directory could not be opened\r\n" );
+ pc().printf("MBED directory could not be opened\r\n");
+ }
+
+ pc().printf("\n\rFiles in SD card directory:\n\r");
+ for ( int i = 0; i < sd_dir.size(); i++ )
+ pc().printf( "%s\r\n", sd_dir[ i ].c_str() );
+}
+
+void StateMachine::printCurrentSdLog() {
+ pc().printf("SD card log work in progress\n\r");
+ //might be worth saving the last few logs to the MBED...
+}
+
+//check if the file is still opened
+void StateMachine::createNewFile() {
+ if (_file_closed) {
+ mbedLogger().createFile(); //create a new MBED file
+
+ _file_closed = false; //file is still open until you get to SIT_IDLE
+ }
+}
+
+void StateMachine::transmitData() {
+ static float transmit_timer = 0;
+ static bool is_transmit_timer_running = false;
+
+ if (!is_transmit_timer_running) {
+ //pc().printf("\n\n\rTRANSMIT timer running...\n\n\r"); //debug
+
+ transmit_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds
+ is_transmit_timer_running = true; //disable this block after one iteration
+
+ pc().printf("TESTING to see if this transmits once a second. (timer: %0.1f)\n\r", timer.read());
+ }
+ if (timer.read() >= transmit_timer) {
+ is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
+ }
}
\ No newline at end of file