Diff: StateMachine/StateMachine.cpp

Revision:

76:c802e1da4179

Parent:

74:d281aaef9766

Child:

80:4e5d306d695b

--- a/StateMachine/StateMachine.cpp	Thu Aug 16 13:57:19 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Thu Aug 16 21:47:40 2018 +0000
@@ -1131,8 +1131,8 @@
             xbee().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\r\n\n",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
             
             xbee().printf("\r\n");
-            xbee().printf("bce      P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, deadband: %0.1f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getDeadband());
-            xbee().printf("batt     P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, deadband: %0.1f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getDeadband());
+            xbee().printf("bce      P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getFilterFrequency(), bce().getDeadband());
+            xbee().printf("batt     P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getFilterFrequency(), batt().getDeadband());
             xbee().printf("rudder   min_pwm:%6.1f, center_pwm:%6.1f, max_pwm:%6.1f (min_deg:%6.1f max_deg:%6.1f)\r\n",rudder().getMinPWM(),rudder().getCenterPWM(),rudder().getMaxPWM(),rudder().getMinDeg(),rudder().getMaxDeg());
             xbee().printf("depth    P:%6.2f, I:%6.2f, D:%6.2f, output offset: %6.1f mm, filter_freq: %0.1f, deadband: %0.1f \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset(),depthLoop().getFilterFrequency(),depthLoop().getDeadband());
             xbee().printf("pitch    P:%6.2f, I:%6.2f, D:%6.2f, output offset: %6.1f mm, filter_freq: %0.1f, deadband: %0.1f \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset(),pitchLoop().getFilterFrequency(),pitchLoop().getDeadband());
@@ -1640,54 +1640,25 @@
         }            
     }
 }
-
-//void StateMachine::keyboard_menu_POSITION_READINGS() {
-//    char TUNING_key;
-//        
-//    // show the menu
-//    xbee().printf("\r\n9: BCE and BMM POSITION READINGS (EXIT WITH 'X' !)");
-//    
-//    while (1) {
-//        if (xbee().readable()) {
-//            TUNING_key = xbee().getc();   //get each keystroke
-//        }
-//        
-//                // 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 {
-//            // Testing out ADC
-//            wait(0.5);
-//            float vref = 5.6;
-//            float vmeasured = 0;
-//            unsigned int raw = adc().readCh5();
-//            vmeasured = ((float)raw)/4095.0*vref;
-//            
-//            xbee().printf("BCE POS(%d), BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d)  << POS: BCE %0.2f, BATT %0.2f >>\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),bce().getPosition_mm(),batt().getPosition_mm());
-//   
-//            continue; // didn't get a user input, so keep waiting for it
-//        }            
-//    }
-//}
  
 void StateMachine::keyboard_menu_BCE_PID_settings() {    
     char BCE_PID_key;
     
-    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
+    // load current values from files
+    float bce_KP = bce().getControllerP();
+    float bce_KI = bce().getControllerI();
+    float bce_KD = bce().getControllerD();
+    
+    float bce_deadband = bce().getDeadband();
+    float bce_frequency = bce().getFilterFrequency();
+    int bce_zero_offset = bce().getZeroCounts(); 
+    //BCE frequency and deadband are hardcoded!
  
     // show the menu
-    xbee().printf("\n\rBuoyancy Engine PID gain settings (MENU)");
-    xbee().printf("\n\rAdjust PID settings with the following keys: P I D. Filter = F, deadband = B.\n\r");
+    xbee().printf("\n\rBuoyancy Engine PID gain settings (MENU). ADJUST WITH CARE!");
+    xbee().printf("\n\rAdjust PID settings with the following keys: P  I  D. Filter = F, deadband = B, zero offset = Z\n\r");
     xbee().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.)\n\n\n\r");
-    xbee().printf("bce    P: %3.3f, I: %3.3f, D %3.3f, zeroOffset %d, limit %3.0f mm, slope %3.3f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+    xbee().printf("bce      P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getFilterFrequency(), bce().getDeadband());    
     
     // handle the key presses
     while(1) {
@@ -1704,11 +1675,16 @@
             // set values
             bce().setControllerP(bce_KP);
             bce().setControllerI(bce_KI);
-            bce().setControllerD(bce_KD);  
+            bce().setControllerD(bce_KD); 
+            
+            bce().setDeadband(bce_deadband);
+            bce().setFilterFrequency(bce_frequency);
+            bce().setZeroCounts(bce_zero_offset); //integer value
  
             // save to "BCE.TXT" file
-            configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD, 100, 6.0, 0.5);
-            xbee().printf("bce    P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+            //saveBattData(float batt_p_gain, float batt_i_gain, float batt_d_gain, int batt_zeroOffset, float batt_filter_freq, float batt_deadband)
+            configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD, bce_zero_offset, bce_frequency, bce_deadband);
+            xbee().printf("bce      P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getFilterFrequency(), bce().getDeadband());
         }
         else if (BCE_PID_key == 'X') {    
             break;  //exit the while loop
@@ -1727,13 +1703,15 @@
         }
         else if (BCE_PID_key == 'F') {
             xbee().printf(">> Type in FILTER FREQUENCY with keyboard.\n\r");
-            float bce_freq = getFloatUserInput();
-            bce().setFilterFrequency(bce_freq);
+            bce_frequency = getFloatUserInput();
         }
         else if (BCE_PID_key == 'B') {
             xbee().printf(">> Type in DEADBAND with keyboard.\n\r");
-            float bce_deadband = getFloatUserInput();
-            bce().setDeadband(bce_deadband);
+            bce_deadband = getFloatUserInput();
+        }
+        else if (BCE_PID_key == 'Z') {
+            xbee().printf(">> Type in zero count offset with keyboard.\n\r");
+            bce_zero_offset = (int)getFloatUserInput();
         }
         else {
             xbee().printf("\n\rBCE: [%c] This key does nothing here.                                  \r", BCE_PID_key);
@@ -1743,17 +1721,23 @@
 
 void StateMachine::keyboard_menu_BATT_PID_settings() {    
     char BMM_PID_key;
+
+    // load current values from files
+    float batt_KP = batt().getControllerP();
+    float batt_KI = batt().getControllerI();
+    float batt_KD = batt().getControllerD();
     
-    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
+    float batt_deadband = batt().getDeadband();
+    float batt_frequency = batt().getFilterFrequency();
+    int batt_zero_offset = batt().getZeroCounts(); 
+    //BATT frequency and deadband are hardcoded!
  
     // print the menu
     xbee().printf("\n\rBattery Motor PID gain settings (MENU)");
     xbee().printf("\n\rAdjust PID settings with the following keys: P I D. Filter = F, deadband = B.\n\r");
     xbee().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.)\n\n\n\r");
-    xbee().printf("batt    P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
-    
+    xbee().printf("batt     P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getFilterFrequency(), batt().getDeadband());
+
     // handle the key presses
     while(1) {
         // get the user's keystroke from either of the two inputs
@@ -1769,11 +1753,16 @@
             // set values
             batt().setControllerP(batt_KP);
             batt().setControllerI(batt_KI);
-            batt().setControllerD(batt_KD);  
+            batt().setControllerD(batt_KD);
+            
+            batt().setDeadband(batt_deadband);
+            batt().setFilterFrequency(batt_frequency);
+            batt().setZeroCounts(batt_zero_offset); //integer value
  
             // save to "BATT.TXT" file
-            configFileIO().saveBattData(batt_KP, batt_KI, batt_KD, 100, 6.0, 0.5);
-            xbee().printf("batt    P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
+            //saveBCEData(float bce_p_gain, float bce_i_gain, float bce_d_gain, int bce_zeroOffset, float bce_filter_freq, float bce_deadband)
+            configFileIO().saveBattData(batt_KP, batt_KI, batt_KD, batt_zero_offset, batt_frequency, batt_deadband);
+            xbee().printf("batt     P:%6.2f, I:%6.2f, D:%6.2f,   zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getFilterFrequency(), batt().getDeadband());
         }
         else if (BMM_PID_key == 'X') {    
             break;  //exit the while loop
@@ -1792,13 +1781,15 @@
         }
         else if (BMM_PID_key == 'F') {
             xbee().printf(">> Type in FILTER FREQUENCY with keyboard.\n\r");
-            float batt_freq = getFloatUserInput();
-            batt().setFilterFrequency(batt_freq);
+            batt_frequency = getFloatUserInput();
         }
         else if (BMM_PID_key == 'B') {
             xbee().printf(">> Type in DEADBAND with keyboard.\n\r");
-            float batt_deadband = getFloatUserInput();
-            batt().setDeadband(batt_deadband);
+            batt_deadband = getFloatUserInput();
+        }
+        else if (BMM_PID_key == 'Z') {
+            xbee().printf(">> Type in zero count offset with keyboard.\n\r");
+            batt_zero_offset = (int)getFloatUserInput();
         }
         else {
             xbee().printf("\n\rBATT: [%c] This key does nothing here.                                  \r", BMM_PID_key);
@@ -1812,6 +1803,7 @@
     float depth_KP = depthLoop().getControllerP();       // load current depth value
     float depth_KI = depthLoop().getControllerI();       // load current depth value
     float depth_KD = depthLoop().getControllerD();       // load current depth value
+    
     float depth_freq = depthLoop().getFilterFrequency();
     float depth_deadband = depthLoop().getDeadband();
  
@@ -1837,6 +1829,7 @@
             depthLoop().setControllerP(depth_KP);
             depthLoop().setControllerI(depth_KI);
             depthLoop().setControllerD(depth_KD);
+            
             depthLoop().setFilterFrequency(depth_freq);
             depthLoop().setDeadband(depth_deadband);
  
@@ -1885,7 +1878,7 @@
     float pitch_KP = pitchLoop().getControllerP();       // load current pitch value
     float pitch_KI = pitchLoop().getControllerI();       // load current pitch value
     float pitch_KD = pitchLoop().getControllerD();       // load current pitch value
-    float pitch_offset_deg = pitchLoop().getOutputOffset();
+
     float pitch_freq = pitchLoop().getFilterFrequency();
     float pitch_deadband = pitchLoop().getDeadband();
  
@@ -1911,6 +1904,7 @@
             pitchLoop().setControllerP(pitch_KP);
             pitchLoop().setControllerI(pitch_KI);
             pitchLoop().setControllerD(pitch_KD);
+            
             pitchLoop().setFilterFrequency(pitch_freq);
             pitchLoop().setDeadband(pitch_deadband);
  
@@ -1946,9 +1940,6 @@
             xbee().printf(">> Type in DEADBAND with keyboard.\n\r");
             pitch_deadband = getFloatUserInput();
         }
-        else if (PITCH_PID_key == 'O') {
-            pitch_offset_deg = getFloatUserInput();
-        }
         else {
             xbee().printf("\n\rPITCH: [%c] This key does nothing here.                                  \r", PITCH_PID_key);
         }
@@ -1960,7 +1951,8 @@
     
     float heading_KP = headingLoop().getControllerP();
     float heading_KI = headingLoop().getControllerI();
-    float heading_KD = headingLoop().getControllerD();    
+    float heading_KD = headingLoop().getControllerD(); 
+       
     float heading_offset_deg = headingLoop().getOutputOffset();
     float heading_freq = headingLoop().getFilterFrequency();
     float heading_deadband = headingLoop().getDeadband();