Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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?

UserRevisionLine numberNew 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 }