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
SequenceController/SequenceController.cpp
- Committer:
- joel_ssc
- Date:
- 2019-02-15
- Revision:
- 82:0981b9ada820
- Parent:
- 74:d281aaef9766
- Child:
- 85:dd8176285b6e
File content as of revision 82:0981b9ada820:
#include "SequenceController.hpp" #include "StaticDefs.hpp" SequenceController::SequenceController() { _sequence_counter = 0; } void SequenceController::loadSequence() { xbee().printf("\n\rLoading Dive Sequence File:"); ConfigFile read_sequence_cfg; char value[256]; //read configuration file stored on MBED if (!read_sequence_cfg.read("/local/sequence.txt")) { xbee().printf("\n\rERROR:Failure to read sequence.txt file."); } else { /* Read values from the file until you reach an "exit" character" */ //up to 256 items in the sequence for (int i = 0; i < 256; 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))) { xbee().printf("\n\rsequence %d = %s",i,value); sequenceStructLoaded[i] = process(value); //create the structs using process(string randomstring) } 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; //local struct /* CONVERT STRING TO CHAR ARRAY */ const char *cstr = randomstring.c_str(); /* CONVERT STRING TO CHAR ARRAY */ /* DIVE */ //this can only be in the first position if ((signed int) randomstring.find("dive") != -1) { loadStruct.title = "dive"; loadStruct.state = MULTI_DIVE; //NEW: separate state handles multiple dives } /* DIVE */ /* PITCH */ if ((signed int) randomstring.find("neutral") != -1) { loadStruct.title = "neutral"; xbee().printf("\n\rLOAD neutral. %d", randomstring.find("neutral")); loadStruct.state = FIND_NEUTRAL; } /* PITCH */ /* EXIT */ if ((signed int) randomstring.find("exit") != -1) { loadStruct.title = "exit"; xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r"); loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17) } /* EXIT */ /* DEPTH TO FLOAT */ if ((signed int) 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++; } } loadStruct.depth = atof(depth_array); } } /* DEPTH TO FLOAT */ /* PITCH TO FLOAT */ if ((signed int) 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++; } } loadStruct.pitch = atof(pitch_array); } } /* PITCH TO FLOAT */ /* PAUSE */ if ((signed int) randomstring.find("pause") != -1) { loadStruct.title = "pause"; } /* PAUSE */ /* TIME TO FLOAT */ if ((signed int) 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++) { //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug if (cstr[i] == ',') break; else if (cstr[i] == ';') break; else { //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug time_array[time_counter] = cstr[i]; time_counter++; } } loadStruct.timeout = atof(time_array); } /* TIME TO FLOAT */ // /* EXIT */ // if (randomstring.find("exit") != 0) { // loadStruct.title = "exit"; // xbee().printf("\n\rEXIT."); // } // /* EXIT */ return loadStruct; //each iteration of this returns a completed struct } void SequenceController::sequenceFunction() { //xbee().printf("sequenceFunction\n\r"); //debug (verified it is working correctly) int check_current_state = stateMachine().getState(); xbee().printf("State Machine State: %d\n\r", check_current_state); 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; xbee().printf("_current_state: %d\n\r", _current_state); xbee().printf("_sequence_counter: %d\n\r", _sequence_counter); xbee().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 } } int LegController::getLegState() { return _current_state; } LegController::LegController() { _leg_counter = 0; } int LegController::loadLeg() { xbee().printf("\n\rLoading Leg commands Dive File:"); ConfigFile read_leg_cfg; char value[256]; // char buf[256]; char bux2[256]; static int default_legstruct_loaded = 0; sprintf(bux2,"\n\rin loadleg(): Loading Leg commands Dive File:"); mbedLogger().appendDiagFile(bux2,0); if(default_legstruct_loaded == 0 ) { legStructLoaded[0] = load_def_leg(); // is this right call pointer vs item? legStructLoaded[1].title = "exit"; default_legstruct_loaded = 1; } //read configuration file stored on MBED if (!read_leg_cfg.read("/local/legfile.txt")) { //legfile.txt has exact same format as sequence.txt file,with starting 0=leg,.... and ending 1=exit line xbee().printf("\n\rERROR:Failure to read legfile.txt file."); sprintf(bux2,"\n\rERROR:Failure to read legfile.txt file."); //mbedLogger().appendDiagFile(bux2,0); } else { /* Read values from the file until you reach an "exit" character" */ //up to 256 items in the sequence for (int i = 0; i < 256; 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_leg_cfg.getValue(buf, &value[0], sizeof(value))) { xbee().printf("\n\rsequence %d = %s",i,value); sprintf(bux2, "\n\r leg values sequence %d = %s",i,value); mbedLogger().appendDiagFile(bux2,0); legStructLoaded[i] = process(value); //create the structs using process(string randomstring) } if (legStructLoaded[i].title == "exit") { _number_of_legs = i; //before the exit break; } } } //end of successful read if (_number_of_legs > 0 ) {return 1; } else {return 0; } } legStruct LegController::load_def_leg() { legStruct loadStruct; loadStruct.title = "leg"; loadStruct.state = LEG_POSITION_DIVE; loadStruct.max_depth = 15; loadStruct.min_depth = 5; loadStruct.yo_time = 200; loadStruct.timeout = 1800; loadStruct.heading = 90; return loadStruct; } legStruct LegController::process(string randomstring) { //this is the struct that is loaded from the config variables legStruct loadStruct; //local struct /* CONVERT STRING TO CHAR ARRAY */ const char *cstr = randomstring.c_str(); char bux2[256]; /* CONVERT STRING TO CHAR ARRAY */ /* DIVE */ //this can only be in the first position if ((signed int) randomstring.find("leg") != -1) { loadStruct.title = "leg"; loadStruct.state = LEG_POSITION_DIVE; //NEW: separate state handles multiple dives sprintf(bux2, "\n\r process leg file: found leg label, setting state to LEG_POSITION_DIVE"); mbedLogger().appendDiagFile(bux2,3); } /* DIVE */ /* PITCH */ if ((signed int) randomstring.find("neutral") != -1) { loadStruct.title = "neutral"; xbee().printf("\n\rLOAD neutral. %d", randomstring.find("neutral")); loadStruct.state = FIND_NEUTRAL; } /* PITCH */ /* EXIT */ if ((signed int) randomstring.find("exit") != -1) { loadStruct.title = "exit"; xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r"); loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17) sprintf(bux2, "\n\r process legfile: found exit key \n"); mbedLogger().appendDiagFile(bux2,3); } /* EXIT */ /* max DEPTH TO cycle to */ if ((signed int) randomstring.find("max_depth") != -1) { if (randomstring.find("neutral") || randomstring.find("leg")) { int depth_pos = randomstring.find("max_depth") + 10; //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++; } } loadStruct.max_depth = atof(depth_array); sprintf(bux2, "\n\r process legfile: key=max_depth val=%g \n", atof(depth_array)); mbedLogger().appendDiagFile(bux2,3); } } if ((signed int) randomstring.find("min_depth") != -1) { if (randomstring.find("neutral") || randomstring.find("leg")) { int depth_pos = randomstring.find("min_depth") + 10; //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++; } } loadStruct.min_depth = atof(depth_array); sprintf(bux2, "\n\r process legfile: key=min_depth val=%g \n", atof(depth_array)); mbedLogger().appendDiagFile(bux2,3); } } if ((signed int) randomstring.find("heading") != -1) { if (randomstring.find("neutral") || randomstring.find("leg")) { int depth_pos = randomstring.find("heading") + 8; //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++; } } loadStruct.heading = atof(depth_array); sprintf(bux2, "\n\r process legfile: key=heading val=%g \n", atof(depth_array)); mbedLogger().appendDiagFile(bux2,3); } } /* DEPTH TO FLOAT */ /* PAUSE */ if ((signed int) randomstring.find("pause") != -1) { loadStruct.title = "pause"; } /* PAUSE */ /* TIME TO FLOAT */ if ((signed int) 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++) { //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug if (cstr[i] == ',') break; else if (cstr[i] == ';') break; else { //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug time_array[time_counter] = cstr[i]; time_counter++; } } loadStruct.timeout = atof(time_array); sprintf(bux2, "\n\r process legfile: key=timeout val=%g \n", atof(time_array)); mbedLogger().appendDiagFile(bux2,3); } // yo_time is to avoid a single down/or up segment going on too long - maybe 10 minutes? if ((signed int) randomstring.find("yo_time") != -1) { int time_pos = randomstring.find("yo_time") + 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++) { //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug if (cstr[i] == ',') break; else if (cstr[i] == ';') break; else { //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug time_array[time_counter] = cstr[i]; time_counter++; } } loadStruct.yo_time = atof(time_array); sprintf(bux2, "\n\r process legfile: key=yo_time val=%g \n", atof(time_array)); mbedLogger().appendDiagFile(bux2,3); } /* TIME TO FLOAT */ // /* EXIT */ // if (randomstring.find("exit") != 0) { // loadStruct.title = "exit"; // xbee().printf("\n\rEXIT."); // } // /* EXIT */ return loadStruct; //each iteration this returns a completed struct }