File content as of revision 17:7c16b5671d0e:

#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;
}