Diff: StateMachine/StateMachine.cpp

Revision:

38:83d06c294807

Parent:

37:357e98a929cc

Child:

39:58375ca6b6ff

--- a/StateMachine/StateMachine.cpp	Thu Dec 21 16:07:47 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Thu Dec 21 17:05:35 2017 +0000
@@ -606,8 +606,6 @@
 // output the keyboard menu for user's reference
 void StateMachine::showMenu() {
     pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
-    pc().printf(" G to initiate auto-neutral sequence\r\n");
-    pc().printf(" F to jump to auto-neutral FIND PITCH state\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");
@@ -1015,12 +1013,6 @@
         if (userInput == 'D' or userInput == 'd') {
             _keyboard_state = DIVE;
         }
-        else if (userInput == 'G') {
-            _keyboard_state = FIND_AUTO_NEUTRAL_DEPTH;      //new test 12/7/2017
-        }
-        else if (userInput == 'F') {
-            _keyboard_state = FIND_AUTO_NEUTRAL_PITCH;      //new test 12/7/2017
-        }
         else if (userInput == 'N' or userInput == 'n') {
             _keyboard_state = FIND_NEUTRAL;
         }
@@ -1213,21 +1205,26 @@
             pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
             pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());
             pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
+            
+            pc().printf("\n\r");
+            pc().printf("bce    P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+            pc().printf("batt   P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
+            pc().printf("depth  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
+            pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
         }
         
         //when you read the keyboard successfully, change the state
         _state = _keyboard_state;   //set state at the end of this function
-        pc().printf("\n\n\r ********* KEYBOARD STATE: %d *********\n\n\r", _state);
+        //pc().printf("\n\n\r ********* KEYBOARD STATE: %d *********\n\n\r", _state);
     }
 }
  
- 
 void StateMachine::keyboard_menu_BCE_PID_settings() {    
     char PID_key;
     float gain_step_size = 0.01;    // modify this to change gain step size
-    float KP = bce().getControllerP();  // load current value
-    float KI = bce().getControllerI();  // load current global value
-    float KD = bce().getControllerD();  // load current global value
+    float BCE_KP = bce().getControllerP();  // load current value
+    float BCE_KI = bce().getControllerI();  // load current global value
+    float BCE_KD = bce().getControllerD();  // load current global value
  
     // show the menu
     pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
@@ -1247,37 +1244,38 @@
     
         // handle the user's key input
         if (PID_key == '-') {
-            KP -= gain_step_size;
-            pc().printf("P gain: %0.5f               \r\n", KP);
+            BCE_KP -= gain_step_size;
+            pc().printf("P gain: %0.5f               \r\n", BCE_KP);
         }
         else if (PID_key == '=') {
-            KP += gain_step_size;
-            pc().printf("P gain: %0.5f               \r\n", KP);
+            BCE_KP += gain_step_size;
+            pc().printf("P gain: %0.5f               \r\n", BCE_KP);
         }
         else if (PID_key == '[') {
-            KI -= gain_step_size;
-            pc().printf("I gain: %0.5f               \r\n", KI);
+            BCE_KI -= gain_step_size;
+            pc().printf("I gain: %0.5f               \r\n", BCE_KI);
         }
         else if (PID_key == ']') {
-            KI += gain_step_size;
-            pc().printf("I gain: %0.5f               \r\n", KI);
+            BCE_KI += gain_step_size;
+            pc().printf("I gain: %0.5f               \r\n", BCE_KI);
         }
         else if (PID_key == ';') {
-            KD -= gain_step_size;
-            pc().printf("D gain: %0.5f               \r\n", KD);
+            BCE_KD -= gain_step_size;
+            pc().printf("D gain: %0.5f               \r\n", BCE_KD);
         }
         else if (PID_key == '\'') {
-            KD += gain_step_size;
-            pc().printf("D gain: %0.5f               \r\n", KD);
+            BCE_KD += gain_step_size;
+            pc().printf("D gain: %0.5f               \r\n", BCE_KD);
         }
         else if (PID_key == 'S') { // user wants to save these modified values
             // set values
-            bce().setControllerP(KP);
-            bce().setControllerI(KI);
-            bce().setControllerD(KD);  
+            bce().setControllerP(BCE_KP);
+            bce().setControllerI(BCE_KI);
+            bce().setControllerD(BCE_KD);  
  
-            // save into "PID.cfg"
-            //Config_File_IO().write_manual_position_PID_values_to_config(batt_position_P,batt_position_I,batt_position_D,bce_position_P,bce_position_I,bce_position_D);
+            // save to "BATT.TXT" file
+            configFileIO().saveBCEData(BCE_KP, BCE_KI, BCE_KD);
+            
             break;  //exit the while loop
         }
         else if (PID_key == 'X') {    
@@ -1292,9 +1290,9 @@
 void StateMachine::keyboard_menu_BATT_PID_settings() {    
     char PID_key;
     float gain_step_size = 0.01;    // modify this to change gain step size
-    float KP = batt().getControllerP(); // load current global value
-    float KI = batt().getControllerI(); // load current global value
-    float KD = batt().getControllerD(); // load current global value
+    float batt_KP = batt().getControllerP(); // load current global value
+    float batt_KI = batt().getControllerI(); // load current global value
+    float batt_KD = batt().getControllerD(); // load current global value
  
     // print the menu
     pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
@@ -1314,37 +1312,38 @@
  
         // handle the user's key input
         if (PID_key == '-') {
-            KP -= gain_step_size;
-            pc().printf("\rP gain: %0.5f               ", KP);
+            batt_KP -= gain_step_size;
+            pc().printf("\rP gain: %0.5f               ", batt_KP);
         }
         else if (PID_key == '=') {
-            KP += gain_step_size;
-            pc().printf("\rP gain: %0.5f               ", KP);
+            batt_KP += gain_step_size;
+            pc().printf("\rP gain: %0.5f               ", batt_KP);
         }
         else if (PID_key == '[') {
-            KI -= gain_step_size;
-            pc().printf("\rI gain: %0.5f               ", KI);
+            batt_KI -= gain_step_size;
+            pc().printf("\rI gain: %0.5f               ", batt_KI);
         }
         else if (PID_key == ']') {
-            KI += gain_step_size;
-            pc().printf("\rI gain: %0.5f               ", KI);
+            batt_KI += gain_step_size;
+            pc().printf("\rI gain: %0.5f               ", batt_KI);
         }
         else if (PID_key == ';') {
-            KD -= gain_step_size;
-            pc().printf("\rD gain: %0.5f               ", KD);
+            batt_KD -= gain_step_size;
+            pc().printf("\rD gain: %0.5f               ", batt_KD);
         }
         else if (PID_key == '\'') {
-            KD += gain_step_size;
-            pc().printf("\rD gain: %0.5f               ", KD);
+            batt_KD += gain_step_size;
+            pc().printf("\rD gain: %0.5f               ", batt_KD);
         }
         else if (PID_key == 'S') { // user wants to save the modified values
             // set global values
-            batt().setControllerP(KP);
-            batt().setControllerI(KI);
-            batt().setControllerD(KD);
+            batt().setControllerP(batt_KP);
+            batt().setControllerI(batt_KI);
+            batt().setControllerD(batt_KD);
             
-            // save to "PID.cfg" file
-            //Config_File_IO().write_manual_position_PID_values_to_config(batt_position_P,batt_position_I,batt_position_D,bce_position_P,bce_position_I,bce_position_D);
+            // save to "BATT.TXT" file
+            configFileIO().saveBattData(batt_KP, batt_KI, batt_KD);
+
             break;  //exit the while loop
         }
         else if (PID_key == 'X') {