Diff: StateMachine/StateMachine.cpp

Revision:

54:d4990fb68404

Parent:

53:c0586fe62b01

Child:

55:f4ec445c42fe

--- a/StateMachine/StateMachine.cpp	Thu Jun 07 13:02:08 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Fri Jun 08 13:56:30 2018 +0000
@@ -722,8 +722,9 @@
     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(" 6 STREAM STATUS)\r\n");
+    pc().printf(" 6 STREAM STATUS\r\n");
     pc().printf(" 7 RUDDER SERVO settings menu\r\n");
+    pc().printf(" 8 CHANNEL READINGS\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");
@@ -1133,8 +1134,12 @@
             keyboard_menu_STREAM_STATUS();
         }
         
+        else if (userInput == '7') {
+            keyboard_menu_RUDDER_SERVO_settings();
+        }
+        
         else if (userInput == '8') {
-            keyboard_menu_RUDDER_SERVO_settings();
+            keyboard_menu_CHANNEL_READINGS();
         }
         
         // go to sub-menus for the PID gains (this is blocking)
@@ -1204,7 +1209,7 @@
     char STATUS_key;
         
     // show the menu
-    pc().printf("\n\r1: STATUS DEBUG MENU (EXIT WITH 'X' !)");
+    pc().printf("\n\r1: STATUS DEBUG MENU (EXIT WITH 'X' !)\n\r");
     
     while (1) {
         if (pc().readable()) {
@@ -1214,23 +1219,27 @@
         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("(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 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().getRawPSI(),depth().getRawPSI(),imu().getPitch(),headingLoop().getPosition());
+            
             
-            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("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());
+//            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("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
-            pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
+//            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("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("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
         }
@@ -1322,6 +1331,10 @@
         }
     }
 }
+
+void StateMachine::keyboard_menu_COUNTS_STATUS() {
+    
+}
  
 void StateMachine::keyboard_menu_MANUAL_TUNING() {
     char TUNING_key;
@@ -1332,6 +1345,10 @@
     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
     
@@ -1402,26 +1419,40 @@
             TUNING_key = pc().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;
             
-            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);
+                    
+            pc().printf("BCE POS(%d),BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) \r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4());
+            
+            //pc().printf("vessel pressure(%d) batt voltage(%d) board current(%d) LIMIT: BCE(%d) BMM(%d) (HW reading limit: BCE(%d) BMM (%d) \r\n",adc().readCh5(),adc().readCh6(),adc().readCh7(),bce().getSwitch(), batt().getSwitch(),bce().getSwitchState(),batt().getSwitchState());
+            //pc().printf("LIMIT: BCE(%d) BMM(%d) << HW reading limit: BCE(%d) BMM (%d) >> \r\n",bce().getSwitch(), batt().getSwitch(),bce().getHWSwitchReading(),batt().getHWSwitchReading());
+            //pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
             // End of ADC Test
             
+//            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());
+            
             continue; // didn't get a user input, so keep waiting for it
         }            
     }