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:
- 29:6c030ac0c362
- Parent:
- 28:16c83a2fdefa
--- a/StateMachine/StateMachine.cpp Fri Dec 01 22:43:14 2017 +0000
+++ b/StateMachine/StateMachine.cpp Sat Dec 02 20:36:29 2017 +0000
@@ -51,19 +51,18 @@
// finite state machine ... each state has at least one exit criteria
switch (_state) {
case SIT_IDLE :
- case KEYBOARD:
+ case KEYBOARD :
// there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
if (!_isTimeoutRunning) {
+ pc().printf("\r\n\nstate: SIT_IDLE\r\n");
+ _isTimeoutRunning = true; // reset FSM one-shot actions
+ _isSubStateTimerRunning = false; //reset sub FSM one-shot actions
+
showMenu();
- pc().printf("\r\n\nstate: SIT_IDLE\r\n");
- _isTimeoutRunning = true;
-
- // what is active?
+
+ // what needs to be started?
bce().pause();
batt().pause();
-
- //reset sub FSM
- _isSubStateTimerRunning = false;
}
// how exit?
@@ -74,9 +73,9 @@
// start local state timer and init any other one-shot actions
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n");
+ _isTimeoutRunning = true; // reset FSM one-shot actions
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
- _isTimeoutRunning = true;
// what needs to be started?
bce().unpause();
@@ -91,13 +90,11 @@
if (timer > _timeout) {
pc().printf("EC: timed out\r\n");
_state = FLOAT_BROADCAST;
- timer.reset();
_isTimeoutRunning = false;
}
- else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
- pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read());
+ else if (depthLoop().getPosition() < 2.0) { //in case nose is vertical at surface, the depth sensor will be < 2ft
+ pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r", depthLoop().getPosition(), timer.read());
_state = FLOAT_BROADCAST;
- timer.reset();
_isTimeoutRunning = false;
}
break;
@@ -106,15 +103,15 @@
// start local state timer and init any other one-shot actions
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: FIND_NEUTRAL\n\r");
+ _isTimeoutRunning = true; // reset FSM one-shot actions
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
- _isTimeoutRunning = true;
// what needs to be started?
bce().unpause();
batt().unpause();
- bce().setPosition_mm(_bceFloatPosition);
- batt().setPosition_mm(_neutral_batt_pos_mm); //set battery to close-to-neutral setting from config file
+ bce().setPosition_mm(_bceFloatPosition); //set bce to float to start find_neutral actions
+ batt().setPosition_mm(_neutral_batt_pos_mm); //set battery to the current neutral setting
//first iteration goes into Neutral Finding Sub-FSM
//set the first state of the FSM, and start the sub-FSM
@@ -128,11 +125,11 @@
runNeutralStateMachine();
}
- // how exit? (exit with the timer, if timer still running continue processing sub FSM)
+ // how exit?
+ //timeout elapsed
if (timer > _timeout) {
pc().printf("FN: timed out [time: %0.1f sec]\r\n", timer.read());
- _state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces)
- timer.reset();
+ _state = EMERGENCY_CLIMB;
_isTimeoutRunning = false;
//record this to the NEUTRAL sub-FSM tracker
@@ -140,22 +137,21 @@
_substate_array_counter++;
}
- //what is active? (neutral finding sub-function runs until completion)
- //check if substate returned exit state, if so stop running the sub-FSM
+ //check if sub-fsm returned exit state
else if (runNeutralStateMachine() == NEUTRAL_EXIT) {
- //if successful, FIND_NEUTRAL then goes to RISE
pc().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\n\n\r");
_state = RISE;
+ _isTimeoutRunning = false;
}
- break;
+ break;
case DIVE :
// start local state timer and init any other one-shot actions
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: DIVE\r\n");
+ _isTimeoutRunning = true; // reset FSM one-shot actions
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
- _isTimeoutRunning = true;
// what needs to be started?
bce().unpause();
@@ -174,14 +170,12 @@
// how exit?
if (timer > _timeout) {
pc().printf("DIVE: timed out\n\n\r");
- _state = RISE; //new behavior 11/17/2017
- timer.reset();
+ _state = RISE;
_isTimeoutRunning = false;
}
- else if (depthLoop().getPosition() > depthLoop().getCommand()) {
+ else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) {
pc().printf("DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
_state = RISE;
- timer.reset();
_isTimeoutRunning = false;
}
@@ -190,7 +184,8 @@
bce().setPosition_mm(depthLoop().getOutput());
batt().setPosition_mm(pitchLoop().getOutput());
- if (depthLoop().getPosition() > _max_recorded_depth_dive) { //debug
+ // if surpassed previously recorded max depth
+ if (depthLoop().getPosition() > _max_recorded_depth_dive) {
_max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded
}
break;
@@ -199,18 +194,18 @@
// start local state timer and init any other one-shot actions
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: RISE\r\n");
+ _isTimeoutRunning = true; // reset FSM one-shot actions
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
- _isTimeoutRunning = true;
// what needs to be started?
bce().unpause();
batt().unpause();
// what are the commands?
- depthLoop().setCommand(-1.0); //make sure to get towards the surface (saw issues at LASR pool)
+ depthLoop().setCommand(0.0); // depth command is the surface at 0ft
pitchLoop().setCommand(-_pitchCommand);
- pc().printf("RISE: depth cmd: 0.0\r\n");
+ pc().printf("RISE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
}
@@ -218,29 +213,27 @@
if (timer > _timeout) {
pc().printf("RISE: timed out\r\n");
_state = EMERGENCY_CLIMB;
- timer.reset();
_isTimeoutRunning = false;
}
- else if (depthLoop().getPosition() < 0.5) { //removed depthLoop().getCommand()
+ else if (depthLoop().getPosition() < depthLoop().getCommand() + 0.5) { // including offset for low momentum approaches
pc().printf("RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
_state = FLOAT_BROADCAST;
- timer.reset();
_isTimeoutRunning = false;
}
// 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());
+ bce().setPosition_mm(depthLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
+ batt().setPosition_mm(pitchLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
break;
case FLOAT_LEVEL :
// start local state timer and init any other one-shot actions
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: FLOAT_LEVEL\r\n");
+ _isTimeoutRunning = true; // reset FSM one-shot actions
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
- _isTimeoutRunning = true;
// what needs to be started?
bce().unpause();
@@ -255,28 +248,26 @@
if (timer > _timeout) {
pc().printf("FL: timed out\r\n");
_state = FLOAT_BROADCAST;
- timer.reset();
_isTimeoutRunning = false;
}
- else if (fabs(imu().getPitch() - pitchLoop().getCommand()) < fabs(_pitchTolerance)) { //current tolerance is 5 degrees
- pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), _pitchTolerance);
+ else if (fabs(pitchLoop().getPosition() - pitchLoop().getCommand()) < fabs(_pitchTolerance)) {
+ pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, tolerance: %3.1f mm\r\n",pitchLoop().getPosition(), pitchLoop().getCommand(), _pitchTolerance);
_state = FLOAT_BROADCAST;
- timer.reset();
_isTimeoutRunning = false;
}
// 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());
+ batt().setPosition_mm(pitchLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
break;
case FLOAT_BROADCAST :
// start local state timer and init any other one-shot actions
if (!_isTimeoutRunning) {
pc().printf("\r\n\nstate: FLOAT_BROADCAST\r\n");
+ _isTimeoutRunning = true;
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
- _isTimeoutRunning = true;
// what needs to be started?
bce().unpause();
@@ -291,14 +282,12 @@
if (timer > _timeout) {
pc().printf("FB: timed out\r\n");
_state = SIT_IDLE;
- timer.reset();
_isTimeoutRunning = false;
}
else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
(fabs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) {
- 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());
+ 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();
_isTimeoutRunning = false;
}
@@ -331,21 +320,19 @@
// how exit?
if (timer > _timeout) {
pc().printf("\n\n\rMULTI-DIVE: timed out [time: %0.1f]\n\n\r", timer.read());
- _state = MULTI_RISE; //new behavior 11/17/2017
- timer.reset();
+ _state = MULTI_RISE;
_isTimeoutRunning = false;
}
- else if (depthLoop().getPosition() > depthLoop().getCommand()) {
+ else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) {
pc().printf("MULTI-DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
_state = MULTI_RISE;
- timer.reset();
_isTimeoutRunning = false;
}
// 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());
+ bce().setPosition_mm(depthLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
+ batt().setPosition_mm(pitchLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
break;
case MULTI_RISE :
@@ -367,8 +354,8 @@
//retrieve just pitch command from struct
float sequence_pitchCommand = currentStateStruct.pitch;
- // what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017
- depthLoop().setCommand(0.5);
+ // what are the commands?
+ depthLoop().setCommand(0.0);
pitchLoop().setCommand(-sequence_pitchCommand);
pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
}
@@ -377,17 +364,14 @@
if (timer > _timeout) {
pc().printf("MULTI-RISE: timed out [time: %0.1f]\n\n\r", timer.read());
_state = EMERGENCY_CLIMB;
- timer.reset();
_isTimeoutRunning = false;
-
+
//reset multi-dive sequence to start
_multi_dive_counter = 0;
}
- else if (depthLoop().getPosition() < 0.5) { // depth is less than 0.5 (zero is surface level)
+ else if (depthLoop().getPosition() < depthLoop().getCommand() + 0.5) {
pc().printf("MULTI-RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
-
- //going to next state
- _isTimeoutRunning = false;
+ _isTimeoutRunning = false; //going to one-shot actions next time
//successful dive-rise sequence CONTINUES the multi-dive sequence
_multi_dive_counter++;
@@ -399,7 +383,7 @@
//CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (Float_Level)
if (currentStateStruct.state == FLOAT_LEVEL) {
_state = FLOAT_BROADCAST;
- return;
+ return; // Dan 2017-12-02: why is this return? More likely needs to be break; instead.
}
else
@@ -410,8 +394,8 @@
// 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());
+ bce().setPosition_mm(depthLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
+ batt().setPosition_mm(pitchLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
break;
default :
@@ -423,7 +407,6 @@
if (_previous_state != _state) {
_state_array[_state_array_counter] = _state; //save to state array
_state_array_counter++;
-
_previous_state = _state;
}
}
@@ -460,97 +443,78 @@
int StateMachine::runNeutralStateMachine() {
switch (_substate) {
case NEUTRAL_SINKING :
- //start the 10 second timer
+ //start the timer and do any other one-shot actions
if (!_isSubStateTimerRunning) {
+ _isSubStateTimerRunning = true; //reset the one-shot action entry
+
_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?
- //move piston at start of sequence (retract 2.5 mm)
- bce().setPosition_mm(bce().getSetPosition_mm() - 2.5); //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());
-
- _isSubStateTimerRunning = true; //disable this block after one iteration
+ bce().setPosition_mm(bce().getSetPosition_mm() - 2.5); //move piston at start of sequence
+ pitchLoop().setCommand(0.0); // run pitch outer loop controller
+ pc().printf("NEUTRAL_SINKING: Retracting piston [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition());
}
// 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());
+ if (bce().getPosition_mm() <= 0) { //reached the bce travel limit
+ pc().printf("\r\nNEUTRAL_SINKING: BCE is at travel limit [actual %0.1f mm]\r\n", bce().getPosition_mm());
_substate = NEUTRAL_EXIT;
- _isSubStateTimerRunning = false; // reset the sub state timer
- }
- //once deeper than the commanded setpoint...
- else if (depthLoop().getPosition() > _depthCommand) {
- _substate = NEUTRAL_SLOWLY_RISE; // next state
+ _isSubStateTimerRunning = false; //reset the sub state timer
+ }
+ else if (depthLoop().getPosition() > _depthCommand) { //deeper than the commanded setpoint
+ pc().printf("\r\n\nNEUTRAL_SINKING: Depth exceeded < %3.1f ft [time: %0.1f]\r\n", depthLoop().getCommand(), timer.read());
+ _substate = NEUTRAL_SLOWLY_RISE;
_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! 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
+ batt().setPosition_mm(pitchLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
break;
case NEUTRAL_SLOWLY_RISE:
if (!_isSubStateTimerRunning) {
+ _isSubStateTimerRunning = true; //reset the one-shot action entry
+
_neutral_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds
-
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
-
- // 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);
-
+ bce().setPosition_mm(bce().getSetPosition_mm() + 2); //move piston at start of sequence (extend)
+ pitchLoop().setCommand(0.0); // run pitch outer loop controller
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());
-
- _isSubStateTimerRunning = true; //disable this block after one iteration
}
// how exit?
- //once at full travel limit (setPosition) and haven't yet risen, time to give up and exit
- if (bce().getSetPosition_mm() >= bce().getTravelLimit()) {
+ if (bce().getSetPosition_mm() >= bce().getTravelLimit()) { //once at full travel limit
+ pc().printf("\r\nNEUTRAL_SLOWLY_RISE: BCE is at travel limit [actual %0.1f mm]\r\n", bce().getPosition_mm());
_substate = NEUTRAL_EXIT;
_isSubStateTimerRunning = false; // reset the sub state timer
}
- //depth rate or sink rate < 0 ft/s, go to the next substate the next iteration
- else if (depthLoop().getVelocity() < 0) { //less than zero ft/s
+ else if (depthLoop().getVelocity() < 0) { //less than zero ft/s means we're rising
pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 ft/s [time: %0.1f]\r\n", timer.read());
_substate = NEUTRAL_CHECK_PITCH;
_isSubStateTimerRunning = false; // reset the sub state timer
}
// 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());
-
_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
+ batt().setPosition_mm(pitchLoop().getOutput()); //tie the outer loop controller to the inner loop linear actuator controller
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) {
+ _isSubStateTimerRunning = true; //reset the one-shot action entry
+
_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());
@@ -563,49 +527,46 @@
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");
}
-
- _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
+ pc().printf("NEUTRAL_PITCH Found Level!\n\r"); //debug
- if (depthLoop().getPosition() > _max_recorded_depth_neutral) { //debug
+ if (depthLoop().getPosition() > _max_recorded_depth_neutral) {
_max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded
}
+ // found level, but don't save this time
if (_substate == NEUTRAL_FIRST_PITCH) {
- _substate = NEUTRAL_SINKING; // next state starts the sinking
+ pc().printf("NEUTRAL_FIRST_PITCH completed\n\r");
+ _substate = NEUTRAL_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) {
+ pc().printf("NEUTRAL_CHECK_PITCH completed\n\r");
+ _substate = NEUTRAL_EXIT;
+ _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
+
//save positions locally
+ _neutral_bce_pos_mm = bce().getPosition_mm();
_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().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset
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 {