Diff: StateMachine/StateMachine.cpp

Revision:

34:9b66c5188051

Parent:

32:f2f8ae34aadc

Child:

35:2f66ea4863d5

--- a/StateMachine/StateMachine.cpp	Wed Dec 20 13:52:50 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Wed Dec 20 20:24:15 2017 +0000
@@ -55,7 +55,7 @@
 }
  
 //Finite State Machine (FSM)
-void StateMachine::runStateMachine() {         
+int StateMachine::runStateMachine() {         
     // finite state machine ... each state has at least one exit criteria
     switch (_state) {
     case SIT_IDLE :
@@ -98,14 +98,14 @@
             batt().setPosition_mm(0.0);
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            ////////createNewFile();
             
             //show that this is the start of a new EMERGENCY_CLIMB sequence            
-            mbedLogger().saveSequenceStringToFile("EMERGENCY_CLIMB");
             recordState(_state);
             
             //this was missing
             _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
         
         // how exit?
@@ -116,14 +116,16 @@
             _isTimeoutRunning = false;
         }
         else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
-            pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read());
             _state = FLOAT_BROADCAST;
             timer.reset();
             _isTimeoutRunning = false;
         }
         
+        //print status to screen continuously
+        pc().printf("EC: depth: %3.1f (BCE_cmd: %0.1f), pitch: %0.1f deg [%0.1f sec]\r",depthLoop().getPosition(),bce().getPosition_mm(),pitchLoop().getPosition(),timer.read());
+        
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break;
  
@@ -153,14 +155,14 @@
             runNeutralStateMachine(); 
             
             //create the log file (works only if the file is closed)
-            createNewFile();       
+            //createNewFile();       
             
             //show that this is the start of a new FIND_NEUTRAL sequence            
-            mbedLogger().saveSequenceStringToFile("FIND_NEUTRAL");   
             recordState(_state);  
             
             //this was missing
             _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again     
+            recordData(_state);
         }
  
         // how exit? (exit with the timer, if timer still running continue processing sub FSM)
@@ -185,7 +187,7 @@
         }
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break;   
         
@@ -206,25 +208,21 @@
             depthLoop().setCommand(_depth_command);
             pitchLoop().setCommand(_pitch_command);
             
-            //variables to record every 1-4 seconds
-            _depth_command = depthLoop().getCommand();
-            _pitch_command = pitchLoop().getCommand();
-            
-            pc().printf("DIVE: depth cmd: %3.1f\r\n",_depth_command);
-            pc().printf("DIVE: pitch cmd: %3.1f\r\n",_pitch_command);
+            pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
+            pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
             
             //reset max dive depth
             _max_recorded_depth_dive = -99;            //float to record max depth
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            //createNewFile();
             
             //show that this is the start of new dive sequence            
-            mbedLogger().saveSequenceStringToFile("DIVE");
             recordState(_state);
             
             //this was missing
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
  
         // how exit?
@@ -251,7 +249,7 @@
         }
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break;
     
@@ -271,23 +269,19 @@
             // what are the commands?
             depthLoop().setCommand(-1.0);           //make sure to get towards the surface (saw issues at LASR pool)
             pitchLoop().setCommand(-_pitch_command);
-            
-            //variables to record every 1-4 seconds
-            _depth_command = depthLoop().getCommand();
-            _pitch_command = pitchLoop().getCommand();
-            
-            pc().printf("RISE: depth cmd: %0.1f\r\n", _depth_command);
-            pc().printf("RISE: pitch cmd: %0.1f\r\n",_pitch_command);
+                        
+            pc().printf("RISE: depth cmd: %0.1f\r\n",depthLoop().getCommand());
+            pc().printf("RISE: pitch cmd: %0.1f\r\n",pitchLoop().getCommand());
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            //createNewFile();
             
             //show that this is the start of new rise sequence            
-            mbedLogger().saveSequenceStringToFile("RISE");
             recordState(_state);
             
             //this was missing
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
  
         // how exit?
@@ -313,7 +307,7 @@
         batt().setPosition_mm(pitchLoop().getOutput());
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
          
         break;  
     
@@ -334,14 +328,14 @@
             pitchLoop().setCommand(0.0);
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            //createNewFile();
             
             //show that this is the start of a new float level sequence            
-            mbedLogger().saveSequenceStringToFile("FLOAT_LEVEL");
             recordState(_state);
             
             //this was missing
             _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
         
         // how exit?
@@ -363,7 +357,7 @@
         batt().setPosition_mm(pitchLoop().getOutput());
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break;
     
@@ -384,14 +378,14 @@
             batt().setPosition_mm(_battFloatPosition);
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            //createNewFile();
             
             //show that this is the start of a new float broadcast sequence            
-            mbedLogger().saveSequenceStringToFile("BROADCAST");
             recordState(_state);
             
             //this was missing
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
         
         // how exit?
@@ -421,7 +415,7 @@
         pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break;
         
@@ -447,16 +441,16 @@
             pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            //createNewFile();
             
             //show that this is the start of a new MULTI_DIVE sequence            
-            mbedLogger().saveSequenceStringToFile("MULTI_DIVE");
             recordState(_state);  
             
             //no max depth recording right now
             
             //this was missing
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
         
         // how exit?
@@ -479,7 +473,7 @@
         batt().setPosition_mm(pitchLoop().getOutput());
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break;
     
@@ -508,14 +502,14 @@
             pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
             
             //create the log file (works only if the file is closed)
-            createNewFile();
+            //createNewFile();
             
             //show that this is the start of a new MULTI_DIVE sequence            
-            mbedLogger().saveSequenceStringToFile("MULTI_RISE");
             recordState(_state); 
             
             //this was missing
             _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
         
         // how exit?
@@ -544,7 +538,6 @@
             //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (FLOAT_BROADCAST)
             if (currentStateStruct.state == FLOAT_BROADCAST) {
                 _state = FLOAT_BROADCAST;
-                return;
             }
             
             else 
@@ -559,122 +552,33 @@
         batt().setPosition_mm(pitchLoop().getOutput()); 
         
         //record data every 5 seconds
-        recordData();
+        recordData(_state);
         
         break; 
         
-    case FIND_AUTO_NEUTRAL_DEPTH :
-        //RUN TO DEPTH, if this doesn't work, surface; if it does, continue to FIND_AUTO_NEUTRAL_PITCH
-        
-        // start local state timer and init any other one-shot actions
+    case TRANSMIT_DATA :               
         if (!_isTimeoutRunning) {
-            pc().printf("\r\n\nstate: FIND_AUTO_NEUTRAL_DEPTH\r\n");
+            pc().printf("\r\n\nstate: TRANSMIT_DATA\r\n");
             timer.reset(); // timer goes back to zero
             timer.start(); // background timer starts running
             _isTimeoutRunning = true; 
             
             // what needs to be started?
-            bce().unpause();
-            //BATTERY NOT MOVING
  
             // what are the commands?
-            depthLoop().setCommand(_depth_command);
-            pc().printf("FIND_AUTO_NEUTRAL_DEPTH: depth cmd: %3.1f\r\n",depthLoop().getCommand());
-            
-            //reset max dive depth
-            _max_recorded_auto_neutral_depth = -99;            //float to record max depth
-            
-            //this was missing
-            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+            recordData(_state);
         }
  
         // how exit?
-        if (timer > _timeout) {
-            pc().printf("FIND_AUTO_NEUTRAL_DEPTH: timed out (next state: EMERGENCY_CLIMB)\n\n\r");
-            _state = EMERGENCY_CLIMB;
+        if (timer.read() > _timeout) {
+            pc().printf("TRANSMIT_DATA: timed out\n\n\r");
+            _state = SIT_IDLE;
+            timer.reset();
             _isTimeoutRunning = false;
         }
         
-        //transition only when the PV settles and is at depth (then save)
-        else if (depthLoop().getPosition() > depthLoop().getCommand()) {
-            if (depthLoop().getVelocity() < 1.0) {  //less than 1 foot/s
-                //if successful, save the data
-                _neutral_bce_pos_mm = bce().getPosition_mm();    //get the position of the battery motor
-                
-                pc().printf("FIND_AUTO_NEUTRAL_DEPTH: Saving neutral (depth) Buoyancy Engine position: %0.1f mm\n\n\r", _neutral_bce_pos_mm);
-                configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
-                
-                //transition to the next state
-                _state = FIND_AUTO_NEUTRAL_PITCH;
-                _isTimeoutRunning = false;
-            }
-        }
- 
-        // what is active?
-        pc().printf("FIND_AUTO_NEUTRAL_DEPTH: bce pos: %0.1f mm, batt pos: %0.1f mm (depth: %0.1f ft) (pitch: %0.1f deg)[%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
-        bce().setPosition_mm(depthLoop().getOutput());
-        //BATTERY NOT ACTIVE
-        
-        //record max depth for testing
-        if (depthLoop().getPosition() > _max_recorded_auto_neutral_depth) {  //debug
-            _max_recorded_auto_neutral_depth = depthLoop().getPosition();    //new max depth recorded
-        }
-        
-        //record data every 5 seconds
-        recordData();
-        
-        break;
-        
-    case FIND_AUTO_NEUTRAL_PITCH :
-        //RUN TO DEPTH, if this doesn't work, surface; if it does, continue to FIND_AUTO_NEUTRAL_PITCH
-        
-        // start local state timer and init any other one-shot actions
-        if (!_isTimeoutRunning) {
-            pc().printf("\r\n\nstate: FIND_AUTO_NEUTRAL_PITCH\r\n");
-            timer.reset(); // timer goes back to zero
-            timer.start(); // background timer starts running
-            _isTimeoutRunning = true; 
-            
-            // what needs to be started?
-            batt().unpause();
-            bce().pause();      //BUOYANCY ENGINE NOT MOVING
-            
- 
-            // what are the commands?
-            pitchLoop().setCommand(0.0);
-            pc().printf("FIND_AUTO_NEUTRAL_PITCH: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
-            
-            //this was missing
-            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
-        }
- 
-        // how exit?
-        if (timer > _timeout) {
-            pc().printf("FIND_AUTO_NEUTRAL_PITCH: timed out (next state: EMERGENCY_CLIMB)\n\n\r");
-            _state = EMERGENCY_CLIMB;
-            _isTimeoutRunning = false;
-        }
-        
-        //transition only when the PV settles and is at depth (within 2 degrees and rate is less than 2 deg/s) (then save)
-        else if ((fabs(pitchLoop().getPosition()) < 2.0) && (pitchLoop().getVelocity() < 2.0)) {
-            //if successful, save the data
-            _neutral_batt_pos_mm = batt().getPosition_mm();    //get the position of the battery motor
-
-            pc().printf("FIND_AUTO_NEUTRAL_PITCH: Saving neutral (pitch) battery position: %0.1f mm\n\n\r", _neutral_batt_pos_mm);
-            configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
-            
-            //transition to the next state
-            _state = RISE;
-            _isTimeoutRunning = false;
-        }
- 
-        // what is active?
-        pc().printf("FIND_AUTO_NEUTRAL_PITCH: bce pos: %0.1f mm, batt pos: %0.1f mm (depth: %0.1f ft) (pitch: %0.1f deg)[%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
-        batt().setPosition_mm(pitchLoop().getOutput()); //trying to get to zero pitch
-        //BUOYANCY ENGINE NOT ACTIVE
-        
-        //record data every 5 seconds
-        recordData();
+        //action
+        transmitData();
         
         break;
     
@@ -690,6 +594,13 @@
         
         _previous_state = _state;
     }
+    
+    return _state;
+//    //if the state is SIT_IDLE, return 0 / false (for recording)
+//    if (_state == SIT_IDLE)
+//        return 0;
+//    else
+//        return 1;   //return true to indicate that you're recording
 }
  
 // output the keyboard menu for user's reference
@@ -709,6 +620,7 @@
     pc().printf(" Z to show FSM and sub-FSM states.\r\n");
     pc().printf(" P to print the current log file.\r\n");
     pc().printf(" X to print the list of log files.\r\n");
+    pc().printf(" V to transmit data (work in progress).\r\n");
     pc().printf("[/] to change bce neutral position: %0.1f\r\n", _neutral_bce_pos_mm);
     pc().printf("</> to change batt neutral position: %0.1f\r\n", _neutral_batt_pos_mm);
     pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitch_command);
@@ -1137,15 +1049,14 @@
         else if (userInput == 'P') {
             //Print current SD card log file
             printCurrentSdLog();
-            mbedLogger().printFromLogFile();    //print the current log file to the screen
+            mbedLogger().printFromLogFile();        //print the current log file to the screen
         }
         else if (userInput == 'X') {
             printDirectory();
-            //mbedLogger().printDirectory();    //print all log files to the screen
+            //mbedLogger().printDirectory();        //print all log files to the screen
         }
-        else if (userInput == 'C') {
-            //Transmit data (work in progress)
-            transmitData();
+        else if (userInput == 'V') {
+            _keyboard_state = TRANSMIT_DATA;      //Transmit data (work in progress)
         }
 //        else if (userInput == 'H' or userInput == 'h') {
 //            pc().printf("running homing procedure\r\n");
@@ -1633,64 +1544,6 @@
     _timeout = currentStateStruct.timeout;  //set timeout before exiting this function
 }
 
-void StateMachine::recordState(int input_state) {
-    string string_state;
-            
-    for (int i = 0; i < _state_array_counter; i++) {
-        if (input_state == SIT_IDLE)
-            string_state = "SIT_IDLE";
-        else if (input_state == FIND_NEUTRAL)
-            string_state = "FIND_NEUTRAL";
-        else if (input_state == DIVE)
-            string_state = "DIVE";
-        else if (input_state == RISE)
-            string_state = "RISE";
-        else if (input_state == FLOAT_LEVEL)
-            string_state = "FLOAT_LEVEL";
-        else if (input_state == FLOAT_BROADCAST)
-            string_state = "FLOAT_BROADCAST";          
-        else if (input_state == EMERGENCY_CLIMB)
-            string_state = "EMERGENCY_CLIMB";
-        else if (input_state == MULTI_DIVE)
-            string_state = "MULTI_DIVE";
-        else if (input_state == MULTI_RISE)
-            string_state = "MULTI_RISE";
-        else if (input_state == KEYBOARD)
-            string_state = "KEYBOARD";                    
-    }
-    datalogger().printf("%s\n", string_state.c_str());
-}
-
-void StateMachine::recordData() {
-    if (!_is_log_timer_running) {     
-        //pc().printf("\n\n\rlog timer running...\n\n\r");    //debug
-                   
-        _log_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds 
-        _is_log_timer_running = true;    //disable this block after one iteration
-        
-        _data_log[0] = timer.read();                //timer reading (shouldn't this be constant throughout the state?)
-        _data_log[1] = depthLoop().getCommand();    //depth command
-        _data_log[2] = depthLoop().getPosition();   //depth reading
-        _data_log[3] = pitchLoop().getCommand();    //pitch command
-        _data_log[4] = pitchLoop().getPosition();   //pitch reading
-        _data_log[5] = bce().getSetPosition_mm();
-        _data_log[6] = bce().getPosition_mm();
-        _data_log[7] = batt().getSetPosition_mm();
-        _data_log[8] = batt().getPosition_mm();
-        
-        //record data to the MBED every 5 seconds
-        mbedLogger().saveArrayToFile(_data_log);
-        
-        //record data to OpenLog every 5 seconds
-        datalogger().printf("%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f\n",_data_log[0],_data_log[1],
-        _data_log[2],_data_log[3],_data_log[4],_data_log[5],_data_log[6],_data_log[7],_data_log[8]);
-    }
-    if (timer.read() >= _log_timer) {
-        _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
-        //pc().printf("\n\n\r TIME READ >= _log_timer \n\n\r");    //debug
-    }
-}
-
 void StateMachine::printDirectory() {
     //create a DirectoryList object that points to the local directory
     DirectoryList mbed_dir( "/local" );
@@ -1725,7 +1578,7 @@
 //check if the file is still opened
 void StateMachine::createNewFile() {
     if (_file_closed) {
-        mbedLogger().createFile();       //create a new MBED file
+        //mbedLogger().createFile();       //create a new MBED file
         
         _file_closed = false;   //file is still open until you get to SIT_IDLE
     }
@@ -1746,4 +1599,77 @@
     if (timer.read() >= transmit_timer) {
         is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
     }
+}
+
+void StateMachine::recordData(int input_state) {
+    string string_state;
+    if (input_state == SIT_IDLE)
+        string_state = "SIT_IDLE";
+    else if (input_state == FIND_NEUTRAL)
+        string_state = "FIND_NEUTRAL";
+    else if (input_state == DIVE)
+        string_state = "DIVE";
+    else if (input_state == RISE)
+        string_state = "RISE";
+    else if (input_state == FLOAT_LEVEL)
+        string_state = "FLOAT_LEVEL";
+    else if (input_state == FLOAT_BROADCAST)
+        string_state = "FLOAT_BROADCAST";          
+    else if (input_state == EMERGENCY_CLIMB)
+        string_state = "EMERGENCY_CLIMB";
+    else if (input_state == MULTI_DIVE)
+        string_state = "MULTI_DIVE";
+    else if (input_state == MULTI_RISE)
+        string_state = "MULTI_RISE";
+    else if (input_state == KEYBOARD)
+        string_state = "KEYBOARD";
+        
+    if (!_is_log_timer_running) {     
+        //pc().printf("\n\n\rlog timer running...\n\n\r");    //debug
+                   
+        _log_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds 
+        _is_log_timer_running = true;    //disable this block after one iteration
+        
+        _data_log[0] = timer.read();                //timer reading (shouldn't this be constant throughout the state?)
+        _data_log[1] = depthLoop().getCommand();    //depth command
+        _data_log[2] = depthLoop().getPosition();   //depth reading
+        _data_log[3] = pitchLoop().getCommand();    //pitch command
+        _data_log[4] = pitchLoop().getPosition();   //pitch reading
+        _data_log[5] = bce().getSetPosition_mm();
+        _data_log[6] = bce().getPosition_mm();
+        _data_log[7] = batt().getSetPosition_mm();
+        _data_log[8] = batt().getPosition_mm();
+        
+        //record data to the MBED every 5 seconds
+        mbedLogger().saveArrayToFile(string_state,input_state,_data_log);
+    }
+    if (timer.read() >= _log_timer) {
+        _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
+    }
+}
+
+void StateMachine::recordState(int input_state) {
+    string string_state;
+            
+    if (input_state == SIT_IDLE)
+        string_state = "SIT_IDLE";
+    else if (input_state == FIND_NEUTRAL)
+        string_state = "FIND_NEUTRAL";
+    else if (input_state == DIVE)
+        string_state = "DIVE";
+    else if (input_state == RISE)
+        string_state = "RISE";
+    else if (input_state == FLOAT_LEVEL)
+        string_state = "FLOAT_LEVEL";
+    else if (input_state == FLOAT_BROADCAST)
+        string_state = "FLOAT_BROADCAST";          
+    else if (input_state == EMERGENCY_CLIMB)
+        string_state = "EMERGENCY_CLIMB";
+    else if (input_state == MULTI_DIVE)
+        string_state = "MULTI_DIVE";
+    else if (input_state == MULTI_RISE)
+        string_state = "MULTI_RISE";
+    else if (input_state == KEYBOARD)
+        string_state = "KEYBOARD";                    
+    //datalogger().printf("%s\n", string_state.c_str());
 }
\ No newline at end of file