Diff: StateMachine/StateMachine.cpp

Revision:

56:48a8a5a65b82

Parent:

55:f4ec445c42fe

Child:

57:ec69651c8c21

--- a/StateMachine/StateMachine.cpp	Tue Jun 12 15:41:51 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Tue Jun 12 16:20:03 2018 +0000
@@ -1224,8 +1224,9 @@
             
             wait(1);         
             
-            pc().printf("(set) BCE POS: %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS: %0.2f )(channel reading: %d) (volt: %0.2f) << %0.2f >> (Pressure: %0.2f (raw: %0.2f) PITCH: %0.2f HEADING: %0.2f)\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(), depthLoop().getPosition(),depth().readADCCounts(),depth().readVoltage(),depth().getPsi(),depth().getRawPSI(),depth().getRawPSI(),imu().getPitch(),imu().getHeading());
+            pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f (unfiltered: %0.2f), PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depth().getRawPSI(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm());
             
+            //pc().printf("(set) BCE POS: %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS: %0.2f )(channel reading: %d) (volt: %0.2f) << %0.2f >> (Pressure: %0.2f (raw: %0.2f) PITCH: %0.2f HEADING: %0.2f)\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(), depthLoop().getPosition(),depth().readADCCounts(),depth().readVoltage(),depth().getPsi(),depth().getRawPSI(),depth().getRawPSI(),imu().getPitch(),imu().getHeading());
             
             //pc().printf("Neutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
 //            pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
@@ -1244,7 +1245,6 @@
 //            pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
 //            pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());   
             
-            
             continue; // didn't get a user input, so keep waiting for it
         }
     
@@ -1339,30 +1339,36 @@
 void StateMachine::keyboard_menu_COUNTS_STATUS() {
     
 }
- 
+
 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());
+    pc().printf("\n\r1: MANUAL TUNING MENU (EXIT WITH 'X' !) (Pause and Unpause rudder ticker with P and U\n");
+    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, RUDDER with E/R)\n\r");
+    pc().printf("\n\rMANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(), imu().getHeading());
+    
+    //made these into internal parameters
+    float _tuning_bce_pos_mm = 300.0;   //safe starting position
+    float _tuning_batt_pos_mm = 60.0;   //safe starting position
+    float _tuning_rudder_pos_deg = 0.0; //safe starting position
+    
+    //immediately start at those positions
+    bce().setPosition_mm(_tuning_bce_pos_mm);
+    batt().setPosition_mm(_tuning_batt_pos_mm);
+    rudder().setPosition_deg(_tuning_rudder_pos_deg);
     
     // what needs to be started?
-    bce().setPosition_mm(160);
-    
-    batt().setPosition_mm(50);
-    
     bce().unpause();    //this is now active
     batt().unpause();   //this is now active
+    rudder().unpause();
     
-    while (1) {
+    while (1) {                
         if (pc().readable()) {
             TUNING_key = pc().getc();   //get each keystroke
         }
         
-        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());
+        else {            
             continue; // didn't get a user input, so keep waiting for it
         }
     
@@ -1371,40 +1377,138 @@
             // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
             bce().pause();
             batt().pause();
+            rudder().pause();
+            
+            //right now the rudder is always active................................................hmm
+            //deactivate the pin? new/delete?
             
             break;  //exit the while loop
         }
-                
+        
+        //Buoyancy Engine
         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());
+            _tuning_bce_pos_mm = _tuning_bce_pos_mm - 1.0;
+            bce().setPosition_mm(_tuning_bce_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("BCE _tuning_bce_pos_mm now: %0.1f\n\r", _tuning_bce_pos_mm);
+            pc().printf("\n\rMANUAL_TUNING: (BCE CHANGE: %0.1f)\n\r 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());
+            _tuning_bce_pos_mm = _tuning_bce_pos_mm + 1.0;
+            bce().setPosition_mm(_tuning_bce_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("BCE _tuning_bce_pos_mm now: %0.1f\n\r", _tuning_bce_pos_mm);
+            pc().printf("\n\rMANUAL_TUNING: (BCE CHANGE: %0.1f)\n\r 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());
         }
         
+        //BATTERY
         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());
+            _tuning_batt_pos_mm = _tuning_batt_pos_mm - 1.0;
+            batt().setPosition_mm(_tuning_batt_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("\n\rMANUAL_TUNING: (BATT CHANGE: %0.1f)\n\r 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());
+            _tuning_batt_pos_mm = _tuning_batt_pos_mm + 1.0;
+            batt().setPosition_mm(_tuning_batt_pos_mm);              //this variable is loaded from the file at initialization
+            pc().printf("\n\rMANUAL_TUNING: (BATT CHANGE: %0.1f)\n\r 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 == 'c' or TUNING_key == 'C') {
+            pc().printf("MANUAL_TUNING: (current stats) BCE pos: %3.1f mm (cmd: %3.1f mm), BATT pos: %3.1f mm (cmd: %3.1f mm) SERVO: %0.1f deg, %f pwm\r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm());
+        }
+        
+        //RUDER
+        else if (TUNING_key == 'E') {
+            _tuning_rudder_pos_deg = _tuning_rudder_pos_deg - 0.5;
+            rudder().setPosition_deg(_tuning_rudder_pos_deg);
+            pc().printf("MT: RUDDER %0.1f [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\n\r", _tuning_rudder_pos_deg, rudder().getPosition_pwm(), rudder().getPosition_deg(), headingLoop().getPosition(), imu().getHeading());
+        }
+        
+        else if (TUNING_key == 'R') {
+            _tuning_rudder_pos_deg = _tuning_rudder_pos_deg + 0.5;
+            rudder().setPosition_deg(_tuning_rudder_pos_deg);
+            pc().printf("MT: RUDDER %0.1f [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\n\r", _tuning_rudder_pos_deg, rudder().getPosition_pwm(), rudder().getPosition_deg(), headingLoop().getPosition(), imu().getHeading());
+        }
+        
+        else if (TUNING_key == 'P') {
+            rudder().pause();
+        }
+        
+        else if (TUNING_key == 'U') {
+            rudder().unpause();
         }
         
         else {
-            pc().printf("\n\rThis key does nothing here.                                  ");
-        }
-            
+            pc().printf("\n\rMANUAL_TUNING: [%c] This key does nothing here.                                  \r", TUNING_key);
+        }            
     }
 }
+ 
+//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().setPosition_mm(160);
+//    
+//    batt().setPosition_mm(50);
+//    
+//    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 {
+//            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
+//        }
+//    
+//        // 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_CHANNEL_READINGS() {
     char TUNING_key;