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@17:7c16b5671d0e, 2017-11-21 (annotated)
- Committer:
- tnhnrl
- Date:
- Tue Nov 21 22:03:26 2017 +0000
- Revision:
- 17:7c16b5671d0e
- Child:
- 21:38c8544db6f4
FSG code commit 11/21
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tnhnrl | 17:7c16b5671d0e | 1 | #include "SequenceController.hpp" |
tnhnrl | 17:7c16b5671d0e | 2 | #include "StaticDefs.hpp" |
tnhnrl | 17:7c16b5671d0e | 3 | |
tnhnrl | 17:7c16b5671d0e | 4 | SequenceController::SequenceController() { |
tnhnrl | 17:7c16b5671d0e | 5 | pc().baud(57600); //text was garbled before setting this |
tnhnrl | 17:7c16b5671d0e | 6 | |
tnhnrl | 17:7c16b5671d0e | 7 | _sequence_counter = 0; |
tnhnrl | 17:7c16b5671d0e | 8 | } |
tnhnrl | 17:7c16b5671d0e | 9 | |
tnhnrl | 17:7c16b5671d0e | 10 | void SequenceController::loadSequence() { |
tnhnrl | 17:7c16b5671d0e | 11 | pc().printf("\n\rRead NEUTRAL Config File Function"); |
tnhnrl | 17:7c16b5671d0e | 12 | |
tnhnrl | 17:7c16b5671d0e | 13 | ConfigFile read_sequence_cfg; |
tnhnrl | 17:7c16b5671d0e | 14 | char value[256]; |
tnhnrl | 17:7c16b5671d0e | 15 | |
tnhnrl | 17:7c16b5671d0e | 16 | //read configuration file stored on MBED |
tnhnrl | 17:7c16b5671d0e | 17 | if (!read_sequence_cfg.read("/local/sequence.cfg")) { |
tnhnrl | 17:7c16b5671d0e | 18 | pc().printf("\n\rERROR:Failure to read sequence.cfg file."); |
tnhnrl | 17:7c16b5671d0e | 19 | } |
tnhnrl | 17:7c16b5671d0e | 20 | else { |
tnhnrl | 17:7c16b5671d0e | 21 | int array_size = sizeof(_sequence_array)/sizeof(_sequence_array[0]); |
tnhnrl | 17:7c16b5671d0e | 22 | pc().printf("\n\n\rSEQUENCE ARRAY SIZE %d", array_size); |
tnhnrl | 17:7c16b5671d0e | 23 | |
tnhnrl | 17:7c16b5671d0e | 24 | /* Read values from the file until you reach an "exit" character" */ |
tnhnrl | 17:7c16b5671d0e | 25 | //for (int i = 0; i < array_size; i++) { //works |
tnhnrl | 17:7c16b5671d0e | 26 | for (int i = 0; i < 1024; i++) { //works |
tnhnrl | 17:7c16b5671d0e | 27 | /* convert INT to string */ |
tnhnrl | 17:7c16b5671d0e | 28 | char buf[256]; |
tnhnrl | 17:7c16b5671d0e | 29 | sprintf(buf, "%d", i); //searching for 0,1,2,3... |
tnhnrl | 17:7c16b5671d0e | 30 | /* convert INT to string */ |
tnhnrl | 17:7c16b5671d0e | 31 | |
tnhnrl | 17:7c16b5671d0e | 32 | if (read_sequence_cfg.getValue(buf, &value[0], sizeof(value))) { |
tnhnrl | 17:7c16b5671d0e | 33 | pc().printf("\n\rsequence %d = %s",i,value); |
tnhnrl | 17:7c16b5671d0e | 34 | |
tnhnrl | 17:7c16b5671d0e | 35 | sequenceStructLoaded[i] = process(value); //create the structs using process(string randomstring) |
tnhnrl | 17:7c16b5671d0e | 36 | } |
tnhnrl | 17:7c16b5671d0e | 37 | |
tnhnrl | 17:7c16b5671d0e | 38 | |
tnhnrl | 17:7c16b5671d0e | 39 | pc().printf("\n\n\rTitle: %s", sequenceStructLoaded[i].title.c_str()); |
tnhnrl | 17:7c16b5671d0e | 40 | if (sequenceStructLoaded[i].title == "exit") { |
tnhnrl | 17:7c16b5671d0e | 41 | _number_of_sequences = i; //before the exit |
tnhnrl | 17:7c16b5671d0e | 42 | break; |
tnhnrl | 17:7c16b5671d0e | 43 | } |
tnhnrl | 17:7c16b5671d0e | 44 | } |
tnhnrl | 17:7c16b5671d0e | 45 | } //end of successful read |
tnhnrl | 17:7c16b5671d0e | 46 | } |
tnhnrl | 17:7c16b5671d0e | 47 | |
tnhnrl | 17:7c16b5671d0e | 48 | sequenceStruct SequenceController::process(string randomstring) { |
tnhnrl | 17:7c16b5671d0e | 49 | //this is the struct that is loaded from the config variables |
tnhnrl | 17:7c16b5671d0e | 50 | |
tnhnrl | 17:7c16b5671d0e | 51 | sequenceStruct loadStruct; |
tnhnrl | 17:7c16b5671d0e | 52 | |
tnhnrl | 17:7c16b5671d0e | 53 | /* CONVERT STRING TO CHAR ARRAY */ |
tnhnrl | 17:7c16b5671d0e | 54 | const char *cstr = randomstring.c_str(); |
tnhnrl | 17:7c16b5671d0e | 55 | /* CONVERT STRING TO CHAR ARRAY */ |
tnhnrl | 17:7c16b5671d0e | 56 | |
tnhnrl | 17:7c16b5671d0e | 57 | /* DIVE */ |
tnhnrl | 17:7c16b5671d0e | 58 | if (randomstring.find("dive") != -1) { |
tnhnrl | 17:7c16b5671d0e | 59 | loadStruct.title = "dive"; |
tnhnrl | 17:7c16b5671d0e | 60 | loadStruct.state = MULTI_DIVE; //NEW: separate state handles multiple dives |
tnhnrl | 17:7c16b5671d0e | 61 | } |
tnhnrl | 17:7c16b5671d0e | 62 | /* DIVE */ |
tnhnrl | 17:7c16b5671d0e | 63 | |
tnhnrl | 17:7c16b5671d0e | 64 | /* PITCH */ |
tnhnrl | 17:7c16b5671d0e | 65 | if (randomstring.find("neutral") != -1) { |
tnhnrl | 17:7c16b5671d0e | 66 | loadStruct.title = "neutral"; |
tnhnrl | 17:7c16b5671d0e | 67 | loadStruct.state = FIND_NEUTRAL; |
tnhnrl | 17:7c16b5671d0e | 68 | } |
tnhnrl | 17:7c16b5671d0e | 69 | /* PITCH */ |
tnhnrl | 17:7c16b5671d0e | 70 | |
tnhnrl | 17:7c16b5671d0e | 71 | /* EXIT */ |
tnhnrl | 17:7c16b5671d0e | 72 | if (randomstring.find("exit") != -1) { |
tnhnrl | 17:7c16b5671d0e | 73 | loadStruct.title = "exit"; |
tnhnrl | 17:7c16b5671d0e | 74 | loadStruct.state = FLOAT_LEVEL; //this is the normal exit condition of the dive-rise sequence |
tnhnrl | 17:7c16b5671d0e | 75 | } |
tnhnrl | 17:7c16b5671d0e | 76 | /* EXIT */ |
tnhnrl | 17:7c16b5671d0e | 77 | |
tnhnrl | 17:7c16b5671d0e | 78 | /* DEPTH TO FLOAT */ |
tnhnrl | 17:7c16b5671d0e | 79 | if (randomstring.find("depth") != -1) { |
tnhnrl | 17:7c16b5671d0e | 80 | if (randomstring.find("neutral") || randomstring.find("dive")) { |
tnhnrl | 17:7c16b5671d0e | 81 | int depth_pos = randomstring.find("depth") + 6; //11 in example literally "depth=" |
tnhnrl | 17:7c16b5671d0e | 82 | char depth_array[256] = {0}; //clear memory |
tnhnrl | 17:7c16b5671d0e | 83 | int depth_counter = 0; |
tnhnrl | 17:7c16b5671d0e | 84 | for (int i = depth_pos; i < randomstring.length(); i++) { |
tnhnrl | 17:7c16b5671d0e | 85 | if (cstr[i] == ',') |
tnhnrl | 17:7c16b5671d0e | 86 | break; |
tnhnrl | 17:7c16b5671d0e | 87 | else if (cstr[i] == ';') |
tnhnrl | 17:7c16b5671d0e | 88 | break; |
tnhnrl | 17:7c16b5671d0e | 89 | else { |
tnhnrl | 17:7c16b5671d0e | 90 | depth_array[depth_counter] = cstr[i]; |
tnhnrl | 17:7c16b5671d0e | 91 | depth_counter++; |
tnhnrl | 17:7c16b5671d0e | 92 | } |
tnhnrl | 17:7c16b5671d0e | 93 | } |
tnhnrl | 17:7c16b5671d0e | 94 | pc().printf("\n\r(%s) DEPTH: %f", randomstring.c_str(), atof(depth_array)); |
tnhnrl | 17:7c16b5671d0e | 95 | loadStruct.depth = atof(depth_array); |
tnhnrl | 17:7c16b5671d0e | 96 | } |
tnhnrl | 17:7c16b5671d0e | 97 | } |
tnhnrl | 17:7c16b5671d0e | 98 | /* DEPTH TO FLOAT */ |
tnhnrl | 17:7c16b5671d0e | 99 | |
tnhnrl | 17:7c16b5671d0e | 100 | /* PITCH TO FLOAT */ |
tnhnrl | 17:7c16b5671d0e | 101 | if (randomstring.find("pitch") != -1) { |
tnhnrl | 17:7c16b5671d0e | 102 | if (randomstring.find("neutral") || randomstring.find("dive")) { |
tnhnrl | 17:7c16b5671d0e | 103 | int pitch_pos = randomstring.find("pitch") + 6; //11 in example |
tnhnrl | 17:7c16b5671d0e | 104 | char pitch_array[256] = {0}; //clear memory |
tnhnrl | 17:7c16b5671d0e | 105 | int pitch_counter = 0; |
tnhnrl | 17:7c16b5671d0e | 106 | for (int i = pitch_pos; i < randomstring.length(); i++) { |
tnhnrl | 17:7c16b5671d0e | 107 | if (cstr[i] == ',') |
tnhnrl | 17:7c16b5671d0e | 108 | break; |
tnhnrl | 17:7c16b5671d0e | 109 | else if (cstr[i] == ';') |
tnhnrl | 17:7c16b5671d0e | 110 | break; |
tnhnrl | 17:7c16b5671d0e | 111 | else { |
tnhnrl | 17:7c16b5671d0e | 112 | pitch_array[pitch_counter] = cstr[i]; |
tnhnrl | 17:7c16b5671d0e | 113 | pitch_counter++; |
tnhnrl | 17:7c16b5671d0e | 114 | } |
tnhnrl | 17:7c16b5671d0e | 115 | } |
tnhnrl | 17:7c16b5671d0e | 116 | pc().printf("\n\r(%s) PITCH: %f", randomstring.c_str(), atof(pitch_array)); |
tnhnrl | 17:7c16b5671d0e | 117 | loadStruct.pitch = atof(pitch_array); |
tnhnrl | 17:7c16b5671d0e | 118 | } |
tnhnrl | 17:7c16b5671d0e | 119 | } |
tnhnrl | 17:7c16b5671d0e | 120 | /* PITCH TO FLOAT */ |
tnhnrl | 17:7c16b5671d0e | 121 | |
tnhnrl | 17:7c16b5671d0e | 122 | /* PAUSE */ |
tnhnrl | 17:7c16b5671d0e | 123 | if (randomstring.find("pause") != -1) { |
tnhnrl | 17:7c16b5671d0e | 124 | loadStruct.title = "pause"; |
tnhnrl | 17:7c16b5671d0e | 125 | pc().printf("\n\rPAUSE."); |
tnhnrl | 17:7c16b5671d0e | 126 | } |
tnhnrl | 17:7c16b5671d0e | 127 | /* PAUSE */ |
tnhnrl | 17:7c16b5671d0e | 128 | |
tnhnrl | 17:7c16b5671d0e | 129 | /* TIME TO FLOAT */ |
tnhnrl | 17:7c16b5671d0e | 130 | if (randomstring.find("timeout") != -1) { |
tnhnrl | 17:7c16b5671d0e | 131 | int time_pos = randomstring.find("timeout") + 8; //position of timeout + "timeout=" so 8 |
tnhnrl | 17:7c16b5671d0e | 132 | char time_array[256] = {0}; |
tnhnrl | 17:7c16b5671d0e | 133 | int time_counter = 0; |
tnhnrl | 17:7c16b5671d0e | 134 | for (int i = time_pos; i < randomstring.length(); i++) { |
tnhnrl | 17:7c16b5671d0e | 135 | //pc().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug |
tnhnrl | 17:7c16b5671d0e | 136 | |
tnhnrl | 17:7c16b5671d0e | 137 | if (cstr[i] == ',') |
tnhnrl | 17:7c16b5671d0e | 138 | break; |
tnhnrl | 17:7c16b5671d0e | 139 | else if (cstr[i] == ';') |
tnhnrl | 17:7c16b5671d0e | 140 | break; |
tnhnrl | 17:7c16b5671d0e | 141 | else { |
tnhnrl | 17:7c16b5671d0e | 142 | //pc().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug |
tnhnrl | 17:7c16b5671d0e | 143 | time_array[time_counter] = cstr[i]; |
tnhnrl | 17:7c16b5671d0e | 144 | time_counter++; |
tnhnrl | 17:7c16b5671d0e | 145 | } |
tnhnrl | 17:7c16b5671d0e | 146 | } |
tnhnrl | 17:7c16b5671d0e | 147 | pc().printf("\n\r----------- time_array: %s", time_array); |
tnhnrl | 17:7c16b5671d0e | 148 | |
tnhnrl | 17:7c16b5671d0e | 149 | pc().printf("\n\r(%s) \n\rTIMEOUT: %f\n\r", randomstring.c_str(), atof(time_array)); |
tnhnrl | 17:7c16b5671d0e | 150 | loadStruct.timeout = atof(time_array); |
tnhnrl | 17:7c16b5671d0e | 151 | } |
tnhnrl | 17:7c16b5671d0e | 152 | /* TIME TO FLOAT */ |
tnhnrl | 17:7c16b5671d0e | 153 | |
tnhnrl | 17:7c16b5671d0e | 154 | /* EXIT */ |
tnhnrl | 17:7c16b5671d0e | 155 | if (randomstring.find("exit") != -1) { |
tnhnrl | 17:7c16b5671d0e | 156 | loadStruct.title = "exit"; |
tnhnrl | 17:7c16b5671d0e | 157 | pc().printf("\n\rEXIT."); |
tnhnrl | 17:7c16b5671d0e | 158 | } |
tnhnrl | 17:7c16b5671d0e | 159 | /* EXIT */ |
tnhnrl | 17:7c16b5671d0e | 160 | |
tnhnrl | 17:7c16b5671d0e | 161 | return loadStruct; //each iteration this returns a completed struct |
tnhnrl | 17:7c16b5671d0e | 162 | } |
tnhnrl | 17:7c16b5671d0e | 163 | |
tnhnrl | 17:7c16b5671d0e | 164 | void SequenceController::runSequence() { |
tnhnrl | 17:7c16b5671d0e | 165 | //pc().printf("\n\rRUNNING SEQUENCE\n\r"); |
tnhnrl | 17:7c16b5671d0e | 166 | sequenceTicker.attach(this,&SequenceController::sequenceFunction,1.0); |
tnhnrl | 17:7c16b5671d0e | 167 | } |
tnhnrl | 17:7c16b5671d0e | 168 | |
tnhnrl | 17:7c16b5671d0e | 169 | void SequenceController::sequenceFunction() { |
tnhnrl | 17:7c16b5671d0e | 170 | //pc().printf("sequenceFunction\n\r"); //debug (verified it is working correctly) |
tnhnrl | 17:7c16b5671d0e | 171 | |
tnhnrl | 17:7c16b5671d0e | 172 | int check_current_state = stateMachine().getState(); |
tnhnrl | 17:7c16b5671d0e | 173 | pc().printf("State Machine State: %d\n\r", check_current_state); |
tnhnrl | 17:7c16b5671d0e | 174 | pc().printf("State Machine: isTimeoutRunning? %d\n\r", stateMachine().timeoutRunning()); |
tnhnrl | 17:7c16b5671d0e | 175 | |
tnhnrl | 17:7c16b5671d0e | 176 | if (stateMachine().getState() == SIT_IDLE) { |
tnhnrl | 17:7c16b5671d0e | 177 | //system starts idle |
tnhnrl | 17:7c16b5671d0e | 178 | //set the state machine to the current sequence in the array |
tnhnrl | 17:7c16b5671d0e | 179 | //example, set to "dive" and set pitch and depth and timeout |
tnhnrl | 17:7c16b5671d0e | 180 | |
tnhnrl | 17:7c16b5671d0e | 181 | _current_state = sequenceStructLoaded[_sequence_counter].state; |
tnhnrl | 17:7c16b5671d0e | 182 | pc().printf("_current_state: %d\n\r", _current_state); |
tnhnrl | 17:7c16b5671d0e | 183 | pc().printf("_sequence_counter: %d\n\r", _sequence_counter); |
tnhnrl | 17:7c16b5671d0e | 184 | pc().printf("_number_of_sequences: %d\n\r", _number_of_sequences); |
tnhnrl | 17:7c16b5671d0e | 185 | |
tnhnrl | 17:7c16b5671d0e | 186 | stateMachine().setState(_current_state); |
tnhnrl | 17:7c16b5671d0e | 187 | stateMachine().setDepthCommand(sequenceStructLoaded[_sequence_counter].depth); |
tnhnrl | 17:7c16b5671d0e | 188 | stateMachine().setPitchCommand(sequenceStructLoaded[_sequence_counter].pitch); |
tnhnrl | 17:7c16b5671d0e | 189 | stateMachine().setTimeout(sequenceStructLoaded[_sequence_counter].timeout); |
tnhnrl | 17:7c16b5671d0e | 190 | |
tnhnrl | 17:7c16b5671d0e | 191 | if (_sequence_counter == _number_of_sequences-1) //end when you finish all of the sequences |
tnhnrl | 17:7c16b5671d0e | 192 | sequenceTicker.detach(); |
tnhnrl | 17:7c16b5671d0e | 193 | |
tnhnrl | 17:7c16b5671d0e | 194 | _sequence_counter++; //exit ticker when counter complete |
tnhnrl | 17:7c16b5671d0e | 195 | } |
tnhnrl | 17:7c16b5671d0e | 196 | |
tnhnrl | 17:7c16b5671d0e | 197 | //pc().printf("Sequence Controller State: %d\n\r", _current_state); |
tnhnrl | 17:7c16b5671d0e | 198 | } |
tnhnrl | 17:7c16b5671d0e | 199 | |
tnhnrl | 17:7c16b5671d0e | 200 | int SequenceController::getSequenceState() { |
tnhnrl | 17:7c16b5671d0e | 201 | return _current_state; |
tnhnrl | 17:7c16b5671d0e | 202 | } |