Diff: StateMachine/StateMachine.cpp

Revision:

22:a10ee088403b

Parent:

21:38c8544db6f4

Child:

23:434f04ef1fad

--- a/StateMachine/StateMachine.cpp	Wed Nov 22 23:04:11 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Thu Nov 23 02:20:08 2017 +0000
@@ -15,8 +15,9 @@
     
     _neutral_sink_timer = 0;
     _neutral_rise_timer = 0;
+    _level_timer        = 0;
     
-    previousPosition_mm = 300;  // Default is fully extended, loads in find_neutral
+    previousPosition_mm = bce().getTravelLimit();  // Default is bce fully extended, overwritten in FIND_NEUTRAL
  
     _state = SIT_IDLE;              // select starting state here
     isTimeoutRunning = false;       // default timer to not running
@@ -112,9 +113,6 @@
             pc().printf("FN: Neutral sequence, BCE piston at %3.1f\n\r", previousPosition_mm);
             
             //first iteration goes into Neutral Finding Sub-FSM            
-            //_neutral_sub_state_active = true;
-            //_sub_state = runNeutralStateMachine();   //send first state of sub-FSM
-            
             runNeutralStateMachine();   //send first state of sub-FSM
         }
  
@@ -430,7 +428,7 @@
 // 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() {
-    static int substate = NEUTRAL_SINKING;   //to start
+    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());
     
@@ -510,46 +508,74 @@
             }
             break;   
                 
-        case NEUTRAL_CHECK_PITCH :
-            pc().printf("NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \r", timer.read(), pitchLoop().getPosition(), pitchLoop().getVelocity()); //debug  
-            //no timeout in this sub FSM state (find_neutral timer still running)
+        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("\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");
+                if (imu().getPitch() > 2) { // nose is high
+                    batt().setPosition_mm(batt().getPosition_mm() + 1); // move battery forward
+                }
+                else if (imu().getPitch() < -2) { // nose is low
+                    batt().setPosition_mm(batt().getPosition_mm() - 1); // move battery aft
+                }
+            }
  
-            //what are commands?
-            //the BCE piston is maintaining its position 
-            //the battery is maintaining the pitch at zero degrees
-            
             // 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)) { 
-                //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;     //the "case" NEUTRAL_EXIT is used to tell the greater FSM that this sub-FSM has completed
+
+                // found level, but don't need to save anything this time
+                if (substate == NEUTRAL_FIRST_PITCH) {
+                    substate = NEUTRAL_SINKING; // next state starts the sinking
+                }
+                // found level and at depth too, so save it all now
+                else if (_state == 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;
+                }
             }
+            
+            // what is active?
+            //once timer complete, reset the timeout so the state one-shot entry will move the setpoint
+            if (timer.read() >= _level_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
+            }
+
+            pc().printf("NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \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");
+             
+        //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");
             break;
             
-            default :
-                pc().printf("how did we get to substate default?\n\r"); //debug
-                substate = NEUTRAL_EXIT;  //a default within the sub-state machine
-                break;
+        default :
+            pc().printf("how did we get to substate default?\n\r"); //debug
+            substate = NEUTRAL_EXIT;  //a default within the sub-state machine
+            break;
     }
     
     //check if the sub-FSM has completed