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:
- 23:434f04ef1fad
- Parent:
- 22:a10ee088403b
- Child:
- 24:c7d9b5bf3829
--- a/StateMachine/StateMachine.cpp Thu Nov 23 02:20:08 2017 +0000 +++ b/StateMachine/StateMachine.cpp Tue Nov 28 18:00:09 2017 +0000 @@ -37,6 +37,8 @@ _neutral_bce_pos_mm = depthLoop().getOutputOffset(); //load current neutral buoyancy position offset _neutral_batt_pos_mm = pitchLoop().getOutputOffset(); //load current neutral buoyancy position offset + + _state_array_counter = 0; } //Finite State Machine (FSM) @@ -109,8 +111,8 @@ batt().unpause(); //load the current position of the BCE piston into the sub-FSM - previousPosition_mm = bce().getPosition_mm(); - pc().printf("FN: Neutral sequence, BCE piston at %3.1f\n\r", previousPosition_mm); + //previousPosition_mm = bce().getPosition_mm(); + pc().printf("FN: Neutral sequence, BCE piston at %3.1f\n\r", bce().getPosition_mm()); //first iteration goes into Neutral Finding Sub-FSM runNeutralStateMachine(); //send first state of sub-FSM @@ -427,24 +429,28 @@ //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::runNeutralStateMachine() { +int StateMachine::runNeutralStateMachine() { static int substate = NEUTRAL_FIRST_PITCH; //to start - pc().printf("Neutral FSM: %d [time: %0.1f] (depth: %0.1f, pitch: %0.1f)\r", substate, timer.read(), depthLoop().getPosition(), pitchLoop().getPosition()); + //pc().printf("Neutral FSM: substate: %d [time: %0.1f] (depth: %0.1f, pitch: %0.1f, pitch rate: %0.1f) (bce: %0.1f, batt: %0.1f, prev: %0.1f)\r", substate, timer.read(), depthLoop().getPosition(), pitchLoop().getPosition(), pitchLoop().getVelocity(), bce().getPosition_mm(), batt().getPosition_mm(), previousPosition_mm); switch (substate) { case NEUTRAL_SINKING : //start the 10 second timer - if (!isSubStateTimerRunning) { - _neutral_sink_timer = timer.read() + 10; //record the time when this block is first entered and add 10 seconds + if (!isSubStateTimerRunning) { + _neutral_sink_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds pc().printf("\r\n\nSTART NEUTRAL_SINKING (next retraction at %0.1f sec) [current time: %0.1f]\n\r", _neutral_sink_timer, timer.read()); // what are the commands? //move piston at start of sequence (retract 10 mm) - pc().printf("Neutral Sinking: Retracting piston 10 mm\n\r"); - previousPosition_mm -= 10; + previousPosition_mm = bce().getPosition_mm() - 5; bce().setPosition_mm(previousPosition_mm); //no depth command + pc().printf("NEUTRAL_SINKING: Retracting piston 10 mm (sent cmd: %0.1f) [CMD actual: %0.1f]\n\r",previousPosition_mm, bce().getSetPosition_mm()); + + // 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: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); isSubStateTimerRunning = true; //disable this block after one iteration } @@ -452,8 +458,9 @@ // 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 STATE NEUTRAL EXIT)\n\r", bce().getPosition_mm()); substate = NEUTRAL_EXIT; - } + } //once deeper than the commanded setpoint... else if (depthLoop().getPosition() > depthCommand) { substate = NEUTRAL_SLOWLY_RISE; // next state @@ -469,11 +476,12 @@ isSubStateTimerRunning = false; //reset the sub state timer } - + // what is active? + batt().setPosition_mm(pitchLoop().getOutput()); // pitch outer loop is running break; case NEUTRAL_SLOWLY_RISE: - if (!isSubStateTimerRunning) { + if (!isSubStateTimerRunning) { _neutral_rise_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds pc().printf("\r\n\nSTART NEUTRAL_SLOWLY_RISE (next retraction at %0.1f sec) [current time: %0.1f]\r\n",_neutral_rise_timer,timer.read()); @@ -482,9 +490,13 @@ // what are the commands? //move piston at start of sequence (extend) - pc().printf("Neutral Slowly Rise: Extending piston 1 mm\n\r"); - previousPosition_mm += 1; + previousPosition_mm = bce().getPosition_mm() + 2; bce().setPosition_mm(previousPosition_mm); //no depth command + pc().printf("NEUTRAL_SLOWLY_RISE: Extending piston 2 mm (sent cmd: %0.1f) [CMD actual: %0.1f]\n\r",previousPosition_mm, bce().getSetPosition_mm()); + + // 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: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); } // how exit? @@ -495,6 +507,7 @@ //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 pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate below 0 ft/s [time: %0.1f]\r\n", timer.read()); + pc().printf("\r\n\n ###################### NEXT STATE NEUTRAL_CHECK_PITCH ###################### \r\n"); substate = NEUTRAL_CHECK_PITCH; } @@ -506,37 +519,54 @@ isSubStateTimerRunning = false; // reset the sub state timer } + + // what is active? + batt().setPosition_mm(pitchLoop().getOutput()); // pitch outer loop is running 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) { - _level_timer = timer.read() + 5; // record time when this block is entered and add several seconds + //pc().printf("\n\rNEUTRAL_FIRST_PITCH or NEUTRAL_CHECK_PITCH\n\r"); + + if (!isSubStateTimerRunning) { + _level_timer = timer.read() + 10; // record time when this block is entered and add several seconds pc().printf("\r\n\nSTART NEUTRAL_CHECK_PITCH (next move in %0.1f sec)\r\n",_level_timer - timer.read()); isSubStateTimerRunning = true; // out of the initialization block // what are the commands? - pc().printf("Neutral Check Pitch: moving battery in 1mm increments\n\r"); + //pc().printf("Neutral Check Pitch: moving battery in 1mm increments\n\r"); if (imu().getPitch() > 2) { // nose is high batt().setPosition_mm(batt().getPosition_mm() + 1); // move battery forward + pc().printf("\n\rNeutral Check Pitch: moving battery FWD in 1mm increments\n\n\r"); } else if (imu().getPitch() < -2) { // nose is low batt().setPosition_mm(batt().getPosition_mm() - 1); // move battery aft + pc().printf("\n\rNeutral Check Pitch: moving battery AFT in 1mm increments\n\n\r"); } } // how exit? //pitch angle and pitch rate within small tolerance //benchtop tests confirm angle needs to be around 2 degrees - if ((fabs(pitchLoop().getPosition()) < 0.5) and (fabs(pitchLoop().getVelocity()) < 2.0)) { - + if ((fabs(pitchLoop().getPosition()) < 2.0) and (fabs(pitchLoop().getVelocity()) < 5.0)) { + pc().printf("\n\rFound Level (NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH)\n\r"); //debug // found level, but don't need to save anything this time + if (substate == NEUTRAL_FIRST_PITCH) { + pc().printf("\n\rsubstate: NEUTRAL_FIRST_PITCH \n\r"); //debug + substate = NEUTRAL_SINKING; // next state starts the sinking + + pc().printf("\n\rsubstate: %d \n\r", substate); //debug + + // save this neutral (not in file) for pitch + pitchLoop().setOutputOffset(batt().getPosition_mm()); + } - // found level and at depth too, so save it all now - else if (_state == NEUTRAL_CHECK_PITCH) { + + // found level and at depth too, so save it all now + else { //save positions locally _neutral_batt_pos_mm = batt().getPosition_mm(); _neutral_bce_pos_mm = bce().getPosition_mm(); @@ -563,32 +593,39 @@ isSubStateTimerRunning = false; // reset the sub state timer } - pc().printf("NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \r", timer.read(), pitchLoop().getPosition(), pitchLoop().getVelocity()); //debug + //pc().printf("NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \n\r", timer.read(), pitchLoop().getPosition(), pitchLoop().getVelocity()); //debug break; //this state could be removed, it is only used as a transition but is needed to stop entering this function case NEUTRAL_EXIT : //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed - pc().printf("substate: NEUTRAL_EXIT\n\r"); + pc().printf("substate: NEUTRAL_EXIT\n\r"); break; default : pc().printf("how did we get to substate default?\n\r"); //debug - substate = NEUTRAL_EXIT; //a default within the sub-state machine + //a default within the sub-state machine + substate = NEUTRAL_EXIT; break; } //check if the sub-FSM has completed if (substate == NEUTRAL_EXIT) { - //reset substate to NEUTRAL_SINKING (first state) - substate = NEUTRAL_EXIT; + //reset substate to NEUTRAL_FIRST_PITCH (first state) + substate = NEUTRAL_FIRST_PITCH; //indicate to outer FSM that sub-FSM has completed + _state_array[_state_array_counter] = NEUTRAL_EXIT; //record what state the hardware is currently in + _state_array_counter++; //increment at the end of each run + pc().printf("********************************DID YOU EXIT? *******************************\n\n\r"); return NEUTRAL_EXIT; } - else + else { + _state_array[_state_array_counter] = substate; //record what state the hardware is currently in + _state_array_counter++; //increment at the end of each run return substate; //otherwise continue running through sequence + } } // keyboard runs independently of the state machine, handling one key at a time @@ -729,6 +766,11 @@ 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("Neutral sub FSM states: \n\r"); + for (int i = 0; i < _state_array_counter; i++) { + pc().printf("state #%d: %d\n\r", i, _state_array[i]); + } } //when you read the keyboard successfully, change the state