Diff: StateMachine/StateMachine.cpp

Revision:

52:f207567d3ea4

Parent:

51:c5c40272ecc3

Child:

53:c0586fe62b01

--- a/StateMachine/StateMachine.cpp	Mon Jun 04 15:20:21 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Wed Jun 06 19:18:47 2018 +0000
@@ -2,7 +2,7 @@
 #include "StaticDefs.hpp"
  
 StateMachine::StateMachine() {
-    _timeout = 120;            // generic timeout for every state, seconds
+    _timeout = 20;            // generic timeout for every state, seconds
     
     _pitchTolerance = 5.0;     // pitch angle tolerance for FLOAT_LEVEL state
  
@@ -696,7 +696,7 @@
 // output the keyboard menu for user's reference
 void StateMachine::showMenu() {
     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(" T to go into manual tuning mode (This is on a timer!)\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");
@@ -1125,6 +1125,14 @@
             keyboard_menu_MANUAL_TUNING();
         }
         
+        else if (userInput == '6') {
+            keyboard_menu_STREAM_STATUS();
+        }
+        
+        else if (userInput == '8') {
+            keyboard_menu_RUDDER_SERVO_settings();
+        }
+        
         // go to sub-menus for the PID gains (this is blocking)
         else if (userInput == '1') {
             keyboard_menu_BCE_PID_settings();
@@ -1187,6 +1195,129 @@
         //pc().printf("\n\n\r ********* KEYBOARD STATE: %d *********\n\n\r", _state);
     }
 }
+
+void StateMachine::keyboard_menu_STREAM_STATUS() {
+    char STATUS_key;
+        
+    // show the menu
+    pc().printf("\n\r1: STATUS DEBUG MENU (EXIT WITH 'X' !)");
+    
+    while (1) {
+        if (pc().readable()) {
+            STATUS_key = pc().getc();   //get each keystroke
+        }
+        
+        else {
+            
+            wait(1);         
+            pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
+            pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
+            pc().printf("heading (rudder): %3.1f deg\r\n",rudderLoop().getPosition());                   //for heading
+            
+            pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
+            pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
+            
+            pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
+            pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
+            
+            pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
+            pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
+            
+            pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
+            pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());   
+            
+            pc().printf("\n\rNeutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
+            
+            continue; // didn't get a user input, so keep waiting for it
+        }
+    
+        // process the keys            
+        if (STATUS_key == 'X') {               
+            break;  //exit the while loop
+        }
+        
+        else {
+            pc().printf("\n\rThis key (%c) does nothing here.                                  ", STATUS_key);
+        }
+    }
+}
+
+void StateMachine::keyboard_menu_RUDDER_SERVO_settings() {
+    //load current parameters from the rudder
+    float rudder_min_pwm = rudder().getMinPWM();
+    float rudder_max_pwm = rudder().getMaxPWM();
+    float rudder_ctr_pwm = rudder().getCenterPWM();
+    float rudder_min_deg = rudder().getMinDeg();
+    float rudder_max_deg = rudder().getMaxDeg();
+    
+    char RUDDER_PID_key;
+ 
+    // print the menu
+    pc().printf("\n\rRUDDER (servo driver) settings (MENU)");
+    pc().printf("\n\r(Adjust min/max/center PWM settings with the following keys: N and M and C");
+    pc().printf("\n\r(Adjust DEGREE limit settings with the following keys: min = K, max = L");
+    pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.\n\r");
+    pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
+ 
+    // handle the key presses
+    while(1) {
+        // get the user's keystroke from either of the two inputs
+        if (pc().readable()) {
+            RUDDER_PID_key = pc().getc();
+        }
+        else {
+            wait(0.5);
+            pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
+            continue; // didn't get a user input, so keep waiting for it
+        }
+ 
+    // handle the user's key input                
+        if (RUDDER_PID_key == 'S') { // user wants to save the modified values
+            // set global values
+            rudder().setMinPWM(rudder_min_pwm);
+            rudder().setMaxPWM(rudder_max_pwm);
+            rudder().setCenterPWM(rudder_ctr_pwm);
+            rudder().setMinDeg(rudder_min_deg);
+            rudder().setMaxDeg(rudder_max_deg);
+            
+            // save rudder servo driver values for inner loop
+            configFileIO().saveRudderData(rudder_min_deg, rudder_max_deg, rudder_ctr_pwm, rudder_min_pwm, rudder_max_pwm);
+            pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
+        }
+        else if (RUDDER_PID_key == 'X') {    
+            break;  //exit the while loop
+        }
+            // MIN PWM
+        else if (RUDDER_PID_key == 'N') {
+            pc().printf(">> Type in rudder_min_pwm with keyboard.\n\r");
+            rudder_min_pwm = getFloatUserInput();
+        }
+    // MAX PWM
+        else if (RUDDER_PID_key == 'M') {
+            pc().printf(">> Type in rudder_max_pwm with keyboard.\n\r");
+            rudder_max_pwm = getFloatUserInput();
+        }
+    // CENTER PWM
+        else if (RUDDER_PID_key == 'C') {
+            pc().printf(">> Type in rudder_ctr_pwm with keyboard.\n\r");
+            rudder_ctr_pwm = getFloatUserInput();
+        }
+    // MIN DEG
+        else if (RUDDER_PID_key == 'K') {
+            pc().printf(">> Type in rudder_min_deg with keyboard.\n\r");
+            rudder_min_deg = getFloatUserInput();
+        }
+    // MAX DEG
+        else if (RUDDER_PID_key == 'L') {
+            pc().printf(">> Type in rudder_max_deg with keyboard.\n\r");
+            rudder_max_deg = getFloatUserInput();
+        }
+        
+        else {
+            pc().printf("RUDDER SETUP: [%c] This key does nothing here.                           \r", RUDDER_PID_key);
+        }
+    }
+}
  
 void StateMachine::keyboard_menu_MANUAL_TUNING() {
     char TUNING_key;
@@ -1206,6 +1337,7 @@
         }
         
         else {
+            pc().printf("MT: ACTUAL POS (SET POS): BCE: %0.1f (%0.1f), BATT: %0.1f (%0.1f)\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm());
             continue; // didn't get a user input, so keep waiting for it
         }
     
@@ -1224,6 +1356,12 @@
             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 == '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
@@ -1248,6 +1386,42 @@
             
     }
 }
+
+void StateMachine::keyboard_menu_CHANNEL_READINGS() {
+    char TUNING_key;
+        
+    // show the menu
+    pc().printf("\n\r1: CHANNEL READINGS (EXIT WITH 'X' !)");
+    
+    while (1) {
+        if (pc().readable()) {
+            TUNING_key = pc().getc();   //get each keystroke
+        }
+        
+        else {
+            // Testing out ADC
+            float vref = 5.6;
+            float vmeasured = 0;
+            unsigned int raw = adc().readCh5();
+            vmeasured = ((float)raw)/4095.0*vref;
+            
+            pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
+            pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
+            pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
+            pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
+            pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
+            pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
+            pc().printf("raw battery voltage: %d \r\n",adc().readCh6());
+            pc().printf("raw board current: %d \r\n",adc().readCh7());
+            pc().printf("raw BCE limit switch: %d \r\n",bce().getSwitch());
+            pc().printf("raw BMM limit switch: %d \r\n",batt().getSwitch());
+            pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
+            // End of ADC Test
+            
+            continue; // didn't get a user input, so keep waiting for it
+        }            
+    }
+}
  
 void StateMachine::keyboard_menu_BCE_PID_settings() {    
     char PID_key;
@@ -1682,4 +1856,44 @@
 float * StateMachine::dataArray() {
     //return the array to a calling function
     return _data_log;
+}
+
+// 06/06/2018
+float StateMachine::getFloatUserInput() {
+    float float_conversion = 0.0;
+    
+    while(1) {
+        bool valid_input = false;                   //flag for valid or invalid input
+        
+        pc().printf("Please enter your number below and press ENTER:\n\r");
+        char user_string [80];                      //variable to store input as a character array
+    
+        pc().scanf("%s", user_string);              //read formatted data from stdin
+        pc().printf("user_string was <%s>\n\r", user_string);
+        
+        //check through the string for invalid characters (decimal values 43 through 57)
+        for (int c = 0; c < strlen(user_string); c++) {
+            //pc().printf("character is [%c]\n\r", user_string[c]);   //debug
+            if (user_string[c] >= 43 and user_string[c] <= 57) {
+                //pc().printf("VALID CHARACTER!\n\r"); //debug
+                ;
+            }
+            else {
+                pc().printf("INVALID INPUT!\n\r");
+                break;
+            }
+            
+            if (c == (strlen(user_string) - 1)) {
+                valid_input = true;
+            }
+        }
+        
+        if (valid_input) {
+            float_conversion = atof(user_string);
+            pc().printf("VALID INPUT!  Your input was: %f\n\r", float_conversion);
+            break;
+        }
+    }
+    
+    return float_conversion;
 }
\ No newline at end of file