Diff: StateMachine/StateMachine.cpp

Revision:

72:250b2665755c

Parent:

71:939d179478c4

Child:

73:f6f378311c8d

--- a/StateMachine/StateMachine.cpp	Fri Jun 29 21:52:31 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Mon Jul 02 14:28:22 2018 +0000
@@ -64,6 +64,8 @@
  
 //Finite State Machine (FSM)
 int StateMachine::runStateMachine() {       
+    //static int transmit_packet_number = 1;  //for data transmission
+
     // finite state machine ... each state has at least one exit criteria
     switch (_state) {
     case SIT_IDLE :
@@ -693,13 +695,14 @@
             
             //function to read the file and get number of lines (packets to transmit)
             _timeout = mbedLogger().getNumberOfPacketsInCurrentLog();
+            //transmit_packet_number = mbedLogger().getNumberOfPackets();     //for comparison to end transmit sequence from python program
             
             mbedLogger().setTransmitPacketNumber(0); //reset to zero
         }
         
     //TIMED OUT
         if (timer.read() > _timeout) {
-            pc().printf("\r\TX_MBED_LOG: timed out!\r\n");
+            pc().printf("\r\nTX_MBED_LOG: timed out!\r\n");
             
             //STATE
             _state = SIT_IDLE;
@@ -773,8 +776,6 @@
     pc().printf(" G to TX_MBED_LOG\r\n");
     pc().printf(" ~ to erase mbed log file. (clear before logging more than a few runs)\r\n");
     
-    pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
-    pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);  
     pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
     pc().printf("A/S to decrease/increase depth setpoint: %3.1f (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
     
@@ -1065,15 +1066,39 @@
         _isTimeoutRunning = false;
         
         // check command against desired control buttons
+ 
+    /***************************** COMMON COMMANDS *****************************/ 
+        if (user_input == 'W') {
+            pc().printf(">> Please enter the heading (deg).\r\n");
+            _heading_command = getFloatUserInput();
+        }
+        else if (user_input == '8') {
+            keyboard_menu_STREAM_STATUS();
+        }
+        
+        else if (user_input == '|') {
+            pc().printf("taring depth sensor\r\n");
+            pc().printf("Pre-tare:  press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
+            wait(0.1);
+            depth().tare(); // tares to ambient (do on surface)
+            pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
+        }
+            
+        else if (user_input == '?') {
+            pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
+            wait(0.5);
+            mbed_reset();
+        }
+        
+        else if (user_input == 'T') {
+            pc().printf("Please enter the timeout (timer) value below: \n\r");
+            _timeout = fabs(getFloatUserInput());
+        }
+        
+    /***************************** COMMON COMMANDS *****************************/
         
 /***************************** DEBUG MENU *****************************/             
-        if (_debug_menu_on) {
-            
-            if (user_input == 'T') {
-                pc().printf("Please enter the timeout (timer) value below: \n\r");
-                _timeout = fabs(getFloatUserInput());
-            }
-            
+        if (_debug_menu_on) {            
             if (user_input == 'D') {
                 _keyboard_state = DIVE;
             }
@@ -1126,6 +1151,10 @@
             else if (user_input == 'X') {
                 mbedLogger().printMbedDirectory();        //print all log files to the screen
             }
+            else if (user_input == 'G') {
+                wait(1);
+                _keyboard_state = TX_MBED_LOG;
+            }
             else if (user_input == 'I') {
                 mbedLogger().receiveMissionDataWithTicker();    //receive sequence.txt files
             }
@@ -1178,71 +1207,38 @@
                     pc().printf("Neutral Substate #%d: %d (%s)\r\n", i, _state_array[i], string_substate.c_str());
                 }
                 pc().printf("\r\n");  //make space between printouts
-            }
-            else if (user_input == '|') {
-                pc().printf("taring depth sensor\r\n");
-                pc().printf("Pre-tare:  press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
-                wait(0.1);
-                depth().tare(); // tares to ambient (do on surface)
-                pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
-            }
-            
+            }    
+            //BATTERY/PITCH
             else if (user_input == '[' or user_input == '{') {
-                _neutral_bce_pos_mm = depthLoop().getOutputOffset() - 1;
-                depthLoop().setOutputOffset(_neutral_bce_pos_mm); // decrease the bce neutral setpoint
-                pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
-                // save neutral depth value to config file
-                configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
-            }
-            else if (user_input == ']' or user_input == '}') {
-                _neutral_bce_pos_mm = depthLoop().getOutputOffset() + 1;
-                depthLoop().setOutputOffset(_neutral_bce_pos_mm); // increase the bce neutral setpoint
-                pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
-                // save neutral depth value to config file
-                configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
-            }
-            else if (user_input == '<' or user_input == ',') {
-                _neutral_batt_pos_mm = pitchLoop().getOutputOffset() - 1;
+                pc().printf("Please TYPE in the new BATT neutral position.\n\r");
+                _neutral_batt_pos_mm = getFloatUserInput();
                 pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // decrease the batt neutral setpoint
                 pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
                 // save neutral pitch value to config file
                 configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
             }
-            else if (user_input == '>' or user_input == '.') {
-                _neutral_batt_pos_mm = pitchLoop().getOutputOffset() + 1;
-                pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // increase the batt neutral setpoint
-                pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset());
-                // save neutral pitch value to config file
-                configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
-            }
             
-            else if (user_input == '?') {
-                pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
-                wait(0.5);
-                mbed_reset();
+            //BCE/DEPTH
+            else if (user_input == ';' or user_input == ':') {
+                pc().printf("Please TYPE in the new BCE neutral position.\n\r");
+                _neutral_bce_pos_mm = getFloatUserInput();
+                depthLoop().setOutputOffset(_neutral_bce_pos_mm); // decrease the bce neutral setpoint
+                pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset());
+                // save neutral depth value to config file
+                configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
             }
      
-            // change settings        
-            else if (user_input == 'Q' or user_input == 'q') {
-                _pitch_command -= 0.5;         //decrement the pitch setpoint
-                pitchLoop().setCommand(_pitch_command);
-                pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
-            }
-            else if (user_input == 'W' or user_input == 'w') {
-                _pitch_command += 0.5;         //increment the pitch setpoint
-                pitchLoop().setCommand(_pitch_command);
-                pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
+    // change settings
+            //heading is in the common controls        
+            else if (user_input == 'Q') {
+                pc().printf(">> Please enter the desired depth (ft).\r\n");
+                _pitch_command = getFloatUserInput();
             }
-            else if (user_input == 'A' or user_input == 'a') {
-                _depth_command -= 0.5;         //decrement the depth setpoint
-                depthLoop().setCommand(_depth_command);
-                pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
+            else if (user_input == 'A') {
+                pc().printf(">> Please enter the desired pitch (deg).\r\n");
+                _depth_command = getFloatUserInput();
             }
-            else if (user_input == 'S' or user_input == 's') {
-                _depth_command += 0.5;         //increment the depth setpoint
-                depthLoop().setCommand(_depth_command);
-                pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
-            }            
+            
             else if (user_input == '5') {
                 keyboard_menu_RUDDER_SERVO_settings();
             }
@@ -1256,20 +1252,6 @@
                 keyboard_menu_MANUAL_TUNING();
             }
             
-            else if (user_input == '8') {
-                keyboard_menu_STREAM_STATUS();  //and channel readings for debugging
-            }
-            
-            else if (user_input == '9') {
-                _heading_command -= 5.0;         //decrement the rudder setpoint
-                headingLoop().setCommand(_heading_command);
-                pc().printf(">>> (-) new HEADING setpoint: %0.3f deg (-)\r\n", headingLoop().getCommand());
-            }
-            else if (user_input == '0') {
-                _heading_command += 5.0;         //increment the rudder setpoint
-                headingLoop().setCommand(_heading_command);
-                pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
-            }
             else if (user_input == 'U') {
                 keyboard_menu_POSITION_READINGS();
             }
@@ -1329,27 +1311,7 @@
                 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());
-            }
-
-//POSITION DIVE COMMANDS
-            else if (user_input == 'k') {
-                _BCE_dive_offset -= 1.0;
-                pc().printf("Decreased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
-            }
-            else if (user_input == 'l') {
-                _BCE_dive_offset += 1.0;
-                pc().printf("Increased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
-            }
-            else if (user_input == ';') {
-                _BMM_dive_offset -= 1.0;
-                pc().printf("Decreased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
-            }
-            else if (user_input == '\'') {
-                _BMM_dive_offset += 1.0;
-                pc().printf("Increased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
-            }
-//POSITION DIVE COMMANDS
-            
+            }            
             else if (user_input == '*') {
                 pc().printf("SWITCHING TO SIMPLE MENU!\r\n"); 
                 wait(2);
@@ -1359,11 +1321,7 @@
 /***************************** DEBUG MENU *****************************/
             
 /***************************** SIMPLE MENU *****************************/
-        else {
-            if (user_input == 'T') {
-                pc().printf("Please enter the timeout (timer) value below: \n\r");
-                _timeout = fabs(getFloatUserInput());
-            }      
+        else {     
             if (user_input == 'V') {
                 _keyboard_state = POSITION_DIVE;
             }
@@ -1385,12 +1343,11 @@
                 //Print current SD card log file
                 //printCurrentSdLog();
                 mbedLogger().printCurrentLogFile();        //print the current log file to the screen
-            }            
+            }
             else if (user_input == 'G') {
                 wait(1);
                 _keyboard_state = TX_MBED_LOG;
             }
-            
             else if (user_input == '~') {
                 pc().printf("ERASING MBED LOG FILE\r\n"); 
                 mbedLogger().eraseFile();
@@ -1445,65 +1402,17 @@
                 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());
             }
-
-            else if (user_input == 'A' or user_input == 'a') {
-                _depth_command -= 0.5;         //decrement the depth setpoint
-                depthLoop().setCommand(_depth_command);
-                pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
-            }
-            else if (user_input == 'S' or user_input == 's') {
-                _depth_command += 0.5;         //increment the depth setpoint
-                depthLoop().setCommand(_depth_command);
-                pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
-            }
-                        
-            else if (user_input == '?') {
-                pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
-                wait(0.5);
-                mbed_reset();
-            }
-            
-            else if (user_input == '|') {
-                pc().printf("taring depth sensor\r\n");
-                pc().printf("Pre-tare:  press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
-                wait(0.1);
-                depth().tare(); // tares to ambient (do on surface)
-                pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
-            }
             
 //POSITION DIVE COMMANDS
-            else if (user_input == 'k') {
-                _BCE_dive_offset -= 1.0;
-                pc().printf("Decreased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
-            }
-            else if (user_input == 'l') {
-                _BCE_dive_offset += 1.0;
-                pc().printf("Increased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
+            else if (user_input == 'Q') {
+                pc().printf(">> Please enter the desired BMM offset (mm).\r\n");
+                _BMM_dive_offset = getFloatUserInput();
             }
-            else if (user_input == ';') {
-                _BMM_dive_offset -= 1.0;
-                pc().printf("Decreased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
-            }
-            else if (user_input == '\'') {
-                _BMM_dive_offset += 1.0;
-                pc().printf("Increased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
+            else if (user_input == 'A') {
+                pc().printf(">> Please enter the desired BCE offset (mm).\r\n");
+                _BCE_dive_offset = getFloatUserInput();
             }
 //POSITION DIVE COMMANDS
-
-            else if (user_input == '9') {
-                _heading_command -= 5.0;         //decrement the rudder setpoint
-                headingLoop().setCommand(_heading_command);
-                pc().printf(">>> (-) new HEADING setpoint: %0.3f deg (-)\r\n", headingLoop().getCommand());
-            }
-            else if (user_input == '0') {
-                _heading_command += 5.0;         //increment the rudder setpoint
-                headingLoop().setCommand(_heading_command);
-                pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
-            }
-
-            else if (user_input == '8') {
-                keyboard_menu_STREAM_STATUS();
-            }
             
             else if (user_input == '*') {
                 pc().printf("SWITCHING TO DEBUG MENU!\r\n"); 
@@ -2205,28 +2114,6 @@
     }
 }
 
-void StateMachine::transmitData() {
-    static float transmit_timer = 0; 
-    static bool is_transmit_timer_running = false;
-    
-    if (!is_transmit_timer_running) {     
-        //pc().printf("\r\n\nTRANSMIT timer running...\r\n\n");    //debug
-                   
-        transmit_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds 
-        is_transmit_timer_running = true;    //disable this block after one iteration
-        
-        pc().printf("TESTING to see if this transmits once a second. (timer: %0.1f)\r\n", timer.read());
-    }
-    if (timer.read() >= transmit_timer) {
-        is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
-    }
-}
-
-float * StateMachine::dataArray() {
-    //return the array to a calling function
-    return _data_log;
-}
-
 // 06/06/2018
 float StateMachine::getFloatUserInput() {
     float float_conversion = 0.0;