most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
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