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:
- 82:0981b9ada820
- Parent:
- 74:d281aaef9766
- Child:
- 85:dd8176285b6e
--- a/SequenceController/SequenceController.cpp Thu Nov 08 22:30:32 2018 +0000 +++ b/SequenceController/SequenceController.cpp Fri Feb 15 16:00:17 2019 +0000 @@ -149,7 +149,7 @@ // } // /* EXIT */ - return loadStruct; //each iteration this returns a completed struct + return loadStruct; //each iteration of this returns a completed struct } void SequenceController::sequenceFunction() { @@ -180,6 +180,242 @@ } } -int SequenceController::getSequenceState() { +int LegController::getLegState() { return _current_state; -} \ No newline at end of file +} +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 +}