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