Diff: StateMachine/StateMachine.cpp

Revision:

49:47ffa4feb6db

Parent:

45:16b8162188ca

Child:

51:c5c40272ecc3

--- a/StateMachine/StateMachine.cpp	Thu Feb 15 04:01:57 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Thu Feb 15 23:07:25 2018 +0000
@@ -82,6 +82,51 @@
         // how exit?
         keyboard(); // keyboard function will change the state if needed
         break;
+        
+    case CHECK_TUNING :                 // state used to check the tuning of the pressure vessel
+        // start local state timer and init any other one-shot actions
+        if (!_isTimeoutRunning) {
+            pc().printf("\r\n\nstate: CHECK_TUNING\r\n");
+            timer.reset(); // timer goes back to zero
+            timer.start(); // background timer starts running
+            _isTimeoutRunning = true; 
+            
+            // what needs to be started?
+            bce().unpause();    //this is now active
+            batt().unpause();   //this is now active
+ 
+            // what are the commands? (DRIVE THE MOTORS "DIRECTLY")
+            bce().setPosition_mm(_neutral_bce_pos_mm);              //this variable is loaded from the file at initialization
+            batt().setPosition_mm(_neutral_batt_pos_mm);            //this variable is loaded from the file at initialization
+            
+            // getSetPosition_mm is the commanded position in the LinearActuator class
+            
+            pc().printf("CHECK_TUNING: BCE cmd: %3.1f (BCE current position: %3.1f)\r\n", bce().getSetPosition_mm(), bce().getPosition_mm());
+            pc().printf("CHECK_TUNING: BATT cmd: %3.1f (BATT current position: %3.1f)\r\n", batt().getSetPosition_mm(), bce().getPosition_mm());
+
+            //show that this is the start of new dive sequence            
+            recordState(_state);
+            
+            //triggers logger array
+            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
+        }
+    
+        // how exit?
+        if (timer > _timeout) {
+            pc().printf("CHECK_TUNING: timed out!\r\n");
+            _state = FLOAT_BROADCAST;
+            timer.reset();
+            _isTimeoutRunning = false;
+        }
+        
+        //print status to screen continuously
+        pc().printf("CHECK_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (BCE_cmd: %0.1f, BATT_cmd: %0.1f)(depth: %0.1f ft, pitch: %0.1f deg) [%0.1f sec]\r",bce().getPosition_mm(),batt().getPosition_mm(),bce().getSetPosition_mm(),batt().getSetPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(),timer.read());
+        
+        //record data every 5 seconds
+        recordData(_state);
+        
+        break;
  
     case EMERGENCY_CLIMB :
         // start local state timer and init any other one-shot actions
@@ -105,7 +150,7 @@
             //show that this is the start of a new EMERGENCY_CLIMB sequence            
             recordState(_state);
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -162,7 +207,7 @@
             //show that this is the start of a new FIND_NEUTRAL sequence            
             recordState(_state);  
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again     
             recordData(_state);
         }
@@ -222,7 +267,7 @@
             //show that this is the start of new dive sequence            
             recordState(_state);
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -281,7 +326,7 @@
             //show that this is the start of new rise sequence            
             recordState(_state);
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -335,7 +380,7 @@
             //show that this is the start of a new float level sequence            
             recordState(_state);
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -385,7 +430,7 @@
             //show that this is the start of a new float broadcast sequence            
             recordState(_state);
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -414,7 +459,7 @@
         }
         
         // what is active?
-        pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
+        pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
         
         //record data every 5 seconds
         recordData(_state);
@@ -450,7 +495,7 @@
             
             //no max depth recording right now
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -509,7 +554,7 @@
             //show that this is the start of a new MULTI_DIVE sequence            
             recordState(_state); 
             
-            //this was missing
+            //triggers logger array
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
             recordData(_state);
         }
@@ -650,7 +695,8 @@
  
 // output the keyboard menu for user's reference
 void StateMachine::showMenu() {
-    pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
+    pc().printf("\r\r\n\nKEYBOARD MENU (02/15/2018):\r\r\n");
+    pc().printf(" T to go into manual tuning mode (timed)\r\n");
     pc().printf(" N to find neutral\r\n");
     pc().printf(" M to initiate multi-dive cycle\r\n");
     pc().printf(" D to initiate dive cycle\r\n");
@@ -658,12 +704,13 @@
     pc().printf(" L to float level\r\n");
     pc().printf(" B to float at broadcast pitch\r\n");
     pc().printf(" E to initiate emergency climb\r\n");
-    //pc().printf(" H to run homing sequence on both BCE and Batt\r\n");
-    pc().printf(" T to tare the depth sensor\r\n");
+    pc().printf(" '|' to tare the depth sensor (vertical bar)\r\n");
     pc().printf(" Z to show FSM and sub-FSM states.\r\n");
     pc().printf(" P to print the current log file.\r\n");
     pc().printf(" X to print the list of log files.\r\n");
-    pc().printf(" V to transmit data (work in progress).\r\n");
+    pc().printf(" I to receive data.\r\n");
+    pc().printf(" O to transmit data.\r\n");
+    pc().printf(" ~ to erase mbed log file. (clear before logging more than a few runs)\r\n");
     pc().printf("[/] to change bce neutral position: %0.1f\r\n", _neutral_bce_pos_mm);
     pc().printf("</> to change batt neutral position: %0.1f\r\n", _neutral_batt_pos_mm);
     pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitch_command);
@@ -673,6 +720,7 @@
     pc().printf(" 2 BATT PID sub-menu\r\n");
     pc().printf(" 3 Depth PID sub-menu\r\n");
     pc().printf(" 4 Pitch PID sub-menu\r\n");
+    pc().printf(" 5 MANUAL_TUNING sub-menu (does not have a timer!)\r\n");
     pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
     pc().printf(" ? to reset mbed\r\n");
 }
@@ -900,13 +948,13 @@
         _isTimeoutRunning = false;
         
         // check command against desired control buttons
-        if (userInput == 'D' or userInput == 'd') {
+        if (userInput == 'D') {
             _keyboard_state = DIVE;
         }
-        else if (userInput == 'N' or userInput == 'n') {
+        else if (userInput == 'N') {
             _keyboard_state = FIND_NEUTRAL;
         }
-        else if (userInput == 'M' or userInput == 'm') {
+        else if (userInput == 'M') {
             //currently does not run if there is no file.
             
             //need to add method to Sequence Controller that returns -1 
@@ -929,6 +977,12 @@
         else if (userInput == 'E') {
             _keyboard_state = EMERGENCY_CLIMB;
         }
+        
+        else if (userInput == 'T') {
+            _keyboard_state = CHECK_TUNING;
+        }
+        
+        // some debug tools below
         else if (userInput == 'P') {
             //Print current SD card log file
             //printCurrentSdLog();
@@ -940,6 +994,13 @@
         else if (userInput == 'O') {
             _keyboard_state = TRANSMIT_LOG;      //Transmit data (work in progress)
         }
+        else if (userInput == 'I') {
+            mbedLogger().receiveMissionDataWithTicker();    //receive sequence.txt files
+        }
+        else if (userInput == '~') {
+            pc().printf("ERASING MBED LOG FILE\n\r"); 
+            mbedLogger().eraseFile();
+        }
         else if (userInput == 'Z') {
             pc().printf("FSG FSM States: \n\r");
             string string_state;
@@ -986,7 +1047,7 @@
             }
             pc().printf("\n\r");  //make space between printouts
         }
-        else if (userInput == 'T' or userInput == 't') {
+        else if (userInput == '|') {
             pc().printf("taring depth sensor\r\n");
             pc().printf("Pre-tare:  press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
             wait(0.1);
@@ -1059,6 +1120,11 @@
             pc().printf(">>> timeout increased: %d\r\n", _timeout);
         }
         
+        // go to tuning sub-menu
+        else if (userInput == '5') {
+            keyboard_menu_MANUAL_TUNING();
+        }
+        
         // go to sub-menus for the PID gains (this is blocking)
         else if (userInput == '1') {
             keyboard_menu_BCE_PID_settings();
@@ -1103,6 +1169,67 @@
     }
 }
  
+void StateMachine::keyboard_menu_MANUAL_TUNING() {
+    char TUNING_key;
+        
+    // show the menu
+    pc().printf("\n\r1: MANUAL TUNING MENU (EXIT WITH 'X' !)");
+    pc().printf("\n\r(Adjust BCE and BATT positions in real-time.  Timeout NOT running! (decrease/increase BCE with A/S, BATT with Q/W)\n\r");
+    pc().printf("MANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+    
+    // what needs to be started?
+    bce().unpause();    //this is now active
+    batt().unpause();   //this is now active
+    
+    while (1) {
+        if (pc().readable()) {
+            TUNING_key = pc().getc();   //get each keystroke
+        }
+        
+        else {
+            continue; // didn't get a user input, so keep waiting for it
+        }
+    
+        // process the keys            
+        if (TUNING_key == 'X') {    
+            // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
+            bce().pause();
+            batt().pause();
+            
+            break;  //exit the while loop
+        }
+        
+        else if (TUNING_key == 'A') {
+            _neutral_bce_pos_mm = _neutral_bce_pos_mm - 1;
+            bce().setPosition_mm(_neutral_bce_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("MANUAL_TUNING: (BCE CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+        }
+        
+        else if (TUNING_key == 'S') {
+            _neutral_bce_pos_mm = _neutral_bce_pos_mm + 1;
+            bce().setPosition_mm(_neutral_bce_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("MANUAL_TUNING: (BCE CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+        }
+        
+        else if (TUNING_key == 'Q') {
+            _neutral_batt_pos_mm = _neutral_batt_pos_mm - 1;
+            batt().setPosition_mm(_neutral_batt_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("MANUAL_TUNING: (BATT CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+        }
+        
+        else if (TUNING_key == 'W') {
+            _neutral_batt_pos_mm = _neutral_batt_pos_mm + 1;
+            batt().setPosition_mm(_neutral_batt_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("MANUAL_TUNING: (BATT CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+        }
+        
+        else {
+            pc().printf("\n\rThis key does nothing here.                                  ");
+        }
+            
+    }
+}
+ 
 void StateMachine::keyboard_menu_BCE_PID_settings() {    
     char PID_key;
     float gain_step_size = 0.01;    // modify this to change gain step size