Diff: StateMachine/StateMachine.cpp

Revision:

39:58375ca6b6ff

Parent:

38:83d06c294807

Child:

45:16b8162188ca

--- a/StateMachine/StateMachine.cpp	Thu Dec 21 17:05:35 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Thu Dec 21 23:13:44 2017 +0000
@@ -642,11 +642,10 @@
             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());
+                pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (getSetPosition: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition(), bce().getSetPosition_mm());
                 
                 // 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
+                bce().setPosition_mm(bce().getSetPosition_mm() - 2.5);  //Troy: There is some strange error where this has to be a hardcoded number.
                 
                 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());
                 
@@ -673,9 +672,9 @@
                 
                 _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
             }
-            // 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)
+            
+            // what is active? (only the buoyancy engine moved every 5 seconds at start)
+            pc().printf("BCE current pos: %0.1f mm (BCE setpoint: %0.1f mm) (current depth: %0.1f ft)\r", bce().getPosition_mm(),bce().getSetPosition_mm(),depthLoop().getPosition()); //debug
             break;
             
         case NEUTRAL_SLOWLY_RISE:
@@ -686,7 +685,7 @@
                 
                 // what are the commands?
                 //move piston at start of sequence (default: extend 2.0 mm)
-                bce().setPosition_mm(bce().getSetPosition_mm() + _neutral_rise_command_mm);  //no depth command
+                bce().setPosition_mm(bce().getSetPosition_mm() + 2.0);  //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);
@@ -731,11 +730,11 @@
                 
                 // 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)
+                    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 %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)
+                    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 %0.1f mm increments\n\n\r", _neutral_pitch_command_mm);
                 }
 
@@ -831,160 +830,6 @@
         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