Diff: SequenceController/SequenceController.cpp

Revision:

17:7c16b5671d0e

Child:

21:38c8544db6f4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SequenceController/SequenceController.cpp	Tue Nov 21 22:03:26 2017 +0000
@@ -0,0 +1,202 @@
+#include "SequenceController.hpp"
+#include "StaticDefs.hpp"
+
+SequenceController::SequenceController() {
+    pc().baud(57600);   //text was garbled before setting this
+    
+    _sequence_counter = 0;
+}
+
+void SequenceController::loadSequence() {
+    pc().printf("\n\rRead NEUTRAL Config File Function");
+    
+    ConfigFile read_sequence_cfg;
+    char value[256];   
+    
+    //read configuration file stored on MBED
+    if (!read_sequence_cfg.read("/local/sequence.cfg")) {
+        pc().printf("\n\rERROR:Failure to read sequence.cfg file.");
+    }
+    else {
+        int array_size = sizeof(_sequence_array)/sizeof(_sequence_array[0]);
+        pc().printf("\n\n\rSEQUENCE ARRAY SIZE %d", array_size);
+        
+        /* Read values from the file until you reach an "exit" character" */
+        //for (int i = 0; i < array_size; i++) {                  //works
+        for (int i = 0; i < 1024; i++) {                  //works
+            /* convert INT to string */
+            char buf[256];
+            sprintf(buf, "%d", i);                  //searching for 0,1,2,3...
+            /* convert INT to string */
+        
+            if (read_sequence_cfg.getValue(buf, &value[0], sizeof(value))) {
+                pc().printf("\n\rsequence %d = %s",i,value);
+                
+                sequenceStructLoaded[i] = process(value); //create the structs using process(string randomstring)
+            }
+            
+            
+            pc().printf("\n\n\rTitle: %s", sequenceStructLoaded[i].title.c_str());
+            if (sequenceStructLoaded[i].title == "exit") {
+                _number_of_sequences = i;   //before the exit
+                break;
+            }
+        }
+    }   //end of successful read
+}
+
+sequenceStruct SequenceController::process(string randomstring) {
+    //this is the struct that is loaded from the config variables
+    
+    sequenceStruct loadStruct;
+    
+    /* CONVERT STRING TO CHAR ARRAY */
+    const char *cstr = randomstring.c_str();
+    /* CONVERT STRING TO CHAR ARRAY */
+    
+    /* DIVE */
+    if (randomstring.find("dive") != -1) {
+        loadStruct.title = "dive";
+        loadStruct.state = MULTI_DIVE;      //NEW: separate state handles multiple dives
+    }
+    /* DIVE */
+    
+    /* PITCH */
+    if (randomstring.find("neutral") != -1) {
+        loadStruct.title = "neutral";
+        loadStruct.state = FIND_NEUTRAL;
+    }
+    /* PITCH */
+    
+    /* EXIT */
+    if (randomstring.find("exit") != -1) {
+        loadStruct.title = "exit";
+        loadStruct.state = FLOAT_LEVEL; //this is the normal exit condition of the dive-rise sequence
+    }
+    /* EXIT */
+    
+    /* DEPTH TO FLOAT */
+    if (randomstring.find("depth") != -1) {
+        if (randomstring.find("neutral") || randomstring.find("dive")) {
+            int depth_pos = randomstring.find("depth") + 6;     //11 in example literally "depth="
+            char depth_array[256] = {0};    //clear memory
+            int depth_counter = 0;
+            for (int i = depth_pos; i < randomstring.length(); i++) {
+                if (cstr[i] == ',') 
+                    break;
+                else if (cstr[i] == ';') 
+                    break;
+                else {
+                    depth_array[depth_counter] = cstr[i]; 
+                    depth_counter++;
+                }
+            }
+            pc().printf("\n\r(%s) DEPTH: %f", randomstring.c_str(), atof(depth_array));
+            loadStruct.depth = atof(depth_array);
+        }
+    }    
+    /* DEPTH TO FLOAT */
+    
+    /* PITCH TO FLOAT */
+    if (randomstring.find("pitch") != -1) {
+        if (randomstring.find("neutral") || randomstring.find("dive")) {
+            int pitch_pos = randomstring.find("pitch") + 6;     //11 in example
+            char pitch_array[256] = {0};    //clear memory
+            int pitch_counter = 0;
+            for (int i = pitch_pos; i < randomstring.length(); i++) {
+                if (cstr[i] == ',') 
+                    break;
+                else if (cstr[i] == ';') 
+                    break;
+                else {
+                    pitch_array[pitch_counter] = cstr[i]; 
+                    pitch_counter++;
+                }
+            }
+            pc().printf("\n\r(%s) PITCH: %f", randomstring.c_str(), atof(pitch_array));
+            loadStruct.pitch = atof(pitch_array);
+        }
+    }
+    /* PITCH TO FLOAT */
+    
+    /* PAUSE */
+    if (randomstring.find("pause") != -1) {
+        loadStruct.title = "pause";
+        pc().printf("\n\rPAUSE.");
+    }
+    /* PAUSE */
+    
+    /* TIME TO FLOAT */
+    if (randomstring.find("timeout") != -1) {        
+        int time_pos = randomstring.find("timeout") + 8;    //position of timeout + "timeout=" so 8
+        char time_array[256] = {0};
+        int time_counter = 0;
+        for (int i = time_pos; i < randomstring.length(); i++) {
+            //pc().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
+            
+            if (cstr[i] == ',') 
+                break;
+            else if (cstr[i] == ';') 
+                break;
+            else {
+                //pc().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
+                time_array[time_counter] = cstr[i]; 
+                time_counter++;
+            }
+        }
+        pc().printf("\n\r----------- time_array: %s", time_array);
+        
+        pc().printf("\n\r(%s) \n\rTIMEOUT: %f\n\r", randomstring.c_str(), atof(time_array));
+        loadStruct.timeout = atof(time_array);
+    }
+    /* TIME TO FLOAT */  
+    
+    /* EXIT */
+    if (randomstring.find("exit") != -1) {
+        loadStruct.title = "exit";
+        pc().printf("\n\rEXIT.");
+    }
+    /* EXIT */
+    
+    return loadStruct;  //each iteration this returns a completed struct
+}
+
+void SequenceController::runSequence() {
+    //pc().printf("\n\rRUNNING SEQUENCE\n\r");
+    sequenceTicker.attach(this,&SequenceController::sequenceFunction,1.0);
+}
+
+void SequenceController::sequenceFunction() {
+    //pc().printf("sequenceFunction\n\r");    //debug (verified it is working correctly)
+    
+    int check_current_state = stateMachine().getState();
+    pc().printf("State Machine State: %d\n\r", check_current_state);
+    pc().printf("State Machine: isTimeoutRunning? %d\n\r", stateMachine().timeoutRunning());
+        
+    if (stateMachine().getState() == SIT_IDLE) {
+        //system starts idle
+        //set the state machine to the current sequence in the array
+        //example, set to "dive" and set pitch and depth and timeout
+        
+        _current_state = sequenceStructLoaded[_sequence_counter].state;
+        pc().printf("_current_state: %d\n\r", _current_state);
+        pc().printf("_sequence_counter: %d\n\r", _sequence_counter);
+        pc().printf("_number_of_sequences: %d\n\r", _number_of_sequences);
+        
+        stateMachine().setState(_current_state);
+        stateMachine().setDepthCommand(sequenceStructLoaded[_sequence_counter].depth);
+        stateMachine().setPitchCommand(sequenceStructLoaded[_sequence_counter].pitch);
+        stateMachine().setTimeout(sequenceStructLoaded[_sequence_counter].timeout);
+    
+        if (_sequence_counter == _number_of_sequences-1)    //end when you finish all of the sequences       
+            sequenceTicker.detach();
+            
+        _sequence_counter++;        //exit ticker when counter complete
+    }   
+    
+    //pc().printf("Sequence Controller State: %d\n\r", _current_state);
+}
+
+int SequenceController::getSequenceState() {
+    return _current_state;
+}
\ No newline at end of file