SequenceController.cpp

00001 #include "SequenceController.hpp"
00002 #include "StaticDefs.hpp"
00003 
00004 SequenceController::SequenceController() {
00005     _sequence_counter = 0;
00006 }
00007 
00008 void SequenceController::loadSequence() {
00009     xbee().printf("\n\rLoading Dive Sequence File:");
00010     
00011     ConfigFile read_sequence_cfg;
00012     char value[256];
00013     char bux2[256];   
00014     
00015     //read configuration file stored on MBED
00016     if (!read_sequence_cfg.read("/local/sequence.txt")) {
00017         xbee().printf("\n\rERROR:Failure to read sequence.txt file.");
00018     }
00019     else {        
00020         /* Read values from the file until you reach an "exit" character" */
00021         //up to 256 items in the sequence
00022         for (int i = 0; i < 256; i++) {                  //works
00023             /* convert INT to string */
00024             char buf[256];
00025             sprintf(buf, "%d", i);                  //searching for 0,1,2,3...
00026             /* convert INT to string */
00027         
00028             if (read_sequence_cfg.getValue(buf, &value[0], sizeof(value))) {
00029                 xbee().printf("\n\rsequence %d = %s",i,value);
00030                 
00031                 sequenceStructLoaded[i] = process(value); //create the structs using process(string randomstring)
00032             }
00033             
00034             if (sequenceStructLoaded[i].title == "exit") {
00035                 _number_of_sequences = i;   //before the exit
00036                 sprintf(bux2,"\n\rin loadsequence(): found final exit line num-sequences=%d\n", i);
00037                    mbedLogger().appendDiagFile(bux2,0);
00038                 break;
00039             }
00040         }
00041     }   //end of successful read
00042 }
00043 
00044 sequenceStruct SequenceController::process(string randomstring) {
00045     //this is the struct that is loaded from the config variables
00046     char bux2[256];
00047     sequenceStruct loadStruct; //local struct
00048     
00049     /* CONVERT STRING TO CHAR ARRAY */
00050     const char *cstr = randomstring.c_str();
00051     /* CONVERT STRING TO CHAR ARRAY */
00052     
00053     /* DIVE */
00054     //this can only be in the first position
00055     if ((signed int) randomstring.find("dive") != -1) {
00056         loadStruct.title = "dive";
00057         loadStruct.state = MULTI_DIVE;      //NEW: separate state handles multiple dives
00058     }
00059     /* DIVE */
00060     
00061     /* PITCH */
00062     if ((signed int) randomstring.find("neutral") != -1) {
00063         loadStruct.title = "neutral";
00064         xbee().printf("\n\rLOAD neutral. %d", randomstring.find("neutral"));
00065         loadStruct.state = FIND_NEUTRAL;
00066          sprintf(bux2,"\n\rin sequence().process(string): FOUND NEUTRAL in randomstring.find \n");
00067                    mbedLogger().appendDiagFile(bux2,3);
00068     }
00069     /* PITCH */
00070     
00071     /* EXIT */
00072     if ((signed int) randomstring.find("exit") != -1) {
00073         loadStruct.title = "exit";
00074         xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r");
00075         sprintf(bux2,"\n\rin sequence().process(string): FOUND EXIT in randomstring.find \n");
00076                    mbedLogger().appendDiagFile(bux2,0);
00077         loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17)
00078     }
00079     /* EXIT */
00080     
00081     /* DEPTH TO FLOAT */
00082     if ((signed int) randomstring.find("depth") != -1) {
00083         if (randomstring.find("neutral") || randomstring.find("dive")) {
00084             int depth_pos = randomstring.find("depth") + 6;     //11 in example literally "depth="
00085             char depth_array[256] = {0};    //clear memory
00086             int depth_counter = 0;
00087             for (int i = depth_pos; i < randomstring.length(); i++) {
00088                 if (cstr[i] == ',')
00089                     break;
00090                 else if (cstr[i] == ';')
00091                     break;
00092                 else {
00093                     depth_array[depth_counter] = cstr[i];
00094                     depth_counter++;
00095                 }
00096             }
00097             loadStruct.depth = atof(depth_array);
00098         }
00099     }    
00100     /* DEPTH TO FLOAT */
00101     
00102     /* PITCH TO FLOAT */
00103     if ((signed int) randomstring.find("pitch") != -1) {
00104         if (randomstring.find("neutral") || randomstring.find("dive")) {
00105             int pitch_pos = randomstring.find("pitch") + 6;     //11 in example
00106             char pitch_array[256] = {0};    //clear memory
00107             int pitch_counter = 0;
00108             for (int i = pitch_pos; i < randomstring.length(); i++) {
00109                 if (cstr[i] == ',') 
00110                     break;
00111                 else if (cstr[i] == ';') 
00112                     break;
00113                 else {
00114                     pitch_array[pitch_counter] = cstr[i]; 
00115                     pitch_counter++;
00116                 }
00117             }
00118             loadStruct.pitch = atof(pitch_array);
00119         }
00120     }
00121     /* PITCH TO FLOAT */
00122     
00123     /* PAUSE */
00124     if ((signed int) randomstring.find("pause") != -1) {
00125         loadStruct.title = "pause";
00126     }
00127     /* PAUSE */
00128     
00129     /* TIME TO FLOAT */
00130     if ((signed int) randomstring.find("timeout") != -1) { 
00131            
00132         int time_pos = randomstring.find("timeout") + 8;    //position of timeout + "timeout=" so 8
00133         char time_array[256] = {0};
00134         int time_counter = 0;
00135         for (int i = time_pos; i < randomstring.length(); i++) {
00136             //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
00137             
00138             if (cstr[i] == ',') 
00139                 break;
00140             else if (cstr[i] == ';') 
00141                 break;
00142             else {
00143                 //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
00144                 time_array[time_counter] = cstr[i]; 
00145                 time_counter++;
00146             }
00147         }
00148         loadStruct.timeout = atof(time_array);
00149     }
00150     /* TIME TO FLOAT */  
00151     
00152 //    /* EXIT */
00153 //    if (randomstring.find("exit") != 0) {
00154 //        loadStruct.title = "exit";
00155 //        xbee().printf("\n\rEXIT.");
00156 //    }
00157 //    /* EXIT */
00158     
00159     return loadStruct;  //each iteration of this returns a completed struct
00160 }
00161 
00162 void SequenceController::sequenceFunction() {
00163     //xbee().printf("sequenceFunction\n\r");    //debug (verified it is working correctly)
00164     
00165     int check_current_state = stateMachine().getState();
00166     xbee().printf("State Machine State: %d\n\r", check_current_state);
00167         
00168     if (stateMachine().getState() == SIT_IDLE) {
00169         //system starts idle
00170         //set the state machine to the current sequence in the array
00171         //example, set to "dive" and set pitch and depth and timeout
00172         
00173         _current_state = sequenceStructLoaded[_sequence_counter].state;
00174         xbee().printf("_current_state: %d\n\r", _current_state);
00175         xbee().printf("_sequence_counter: %d\n\r", _sequence_counter);
00176         xbee().printf("_number_of_sequences: %d\n\r", _number_of_sequences);
00177         
00178         stateMachine().setState(_current_state);
00179         stateMachine().setDepthCommand(sequenceStructLoaded[_sequence_counter].depth);
00180         stateMachine().setPitchCommand(sequenceStructLoaded[_sequence_counter].pitch);
00181         stateMachine().setTimeout(sequenceStructLoaded[_sequence_counter].timeout);
00182     
00183         if (_sequence_counter == _number_of_sequences-1)    //end when you finish all of the sequences       
00184             sequenceTicker.detach();
00185             
00186         _sequence_counter++;        //exit ticker when counter complete
00187     }   
00188 }
00189 
00190 int LegController::getLegState() {
00191     return _current_state;
00192 }
00193 LegController::LegController() {
00194     _leg_counter = 0;
00195 }
00196 
00197 int LegController::loadLeg() {
00198     xbee().printf("\n\rLoading Leg commands Dive File:");
00199     
00200     ConfigFile read_leg_cfg;
00201     char value[256];   
00202     //   char buf[256];
00203     char bux2[256];
00204     static int default_legstruct_loaded = 0;
00205     sprintf(bux2,"\n\rin loadleg(): Loading Leg commands Dive File:");
00206     mbedLogger().appendDiagFile(bux2,0);
00207     
00208     if(default_legstruct_loaded == 0 ) {
00209         legStructLoaded[0] = load_def_leg();  // is this right call pointer vs item?
00210         legStructLoaded[1].title = "exit";
00211         default_legstruct_loaded = 1;
00212       }
00213     //read configuration file stored on MBED
00214     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
00215         xbee().printf("\n\rERROR:Failure to read legfile.txt file.");
00216         sprintf(bux2,"\n\rERROR:Failure to read legfile.txt file.");
00217         mbedLogger().appendDiagFile(bux2,3);
00218         
00219     }
00220     else {        
00221         /* Read values from the file until you reach an "exit" character" */
00222         //up to 256 items in the sequence
00223         for (int i = 0; i < 256; i++) {                  //works
00224             /* convert INT to string */
00225             char buf[256];
00226             sprintf(buf, "%d", i);                  //searching for 0,1,2,3...
00227             /* convert INT to string */
00228         
00229             if (read_leg_cfg.getValue(buf, &value[0], sizeof(value))) {
00230                 xbee().printf("\n\rsequence %d = %s",i,value);
00231                 sprintf(bux2, "\n\r leg values sequence %d = %s\n",i,value);
00232                 mbedLogger().appendDiagFile(bux2,0);
00233                 
00234                 legStructLoaded[i] = process(value); //create the structs using process(string randomstring)
00235             }
00236             
00237             if (legStructLoaded[i].title == "exit") {
00238                 _number_of_legs = i;   //before the exit
00239                 break;
00240             }
00241         }
00242     }   //end of successful read
00243     if (_number_of_legs  > 0 )  {return 1; }
00244     else {return 0; }
00245 }
00246 
00247 legStruct LegController::load_def_leg() {  // default leg structure and if used, will NOT start leg mode
00248     legStruct loadStruct;
00249     
00250     loadStruct.title = "sit_idle";
00251     loadStruct.state = SIT_IDLE ;    //    LEG_POSITION_DIVE;
00252     loadStruct.max_depth = 15;
00253     loadStruct.min_depth =  5;
00254     loadStruct.yo_time =  100;
00255     loadStruct.timeout =  600;
00256     loadStruct.heading =  90; 
00257     return loadStruct;
00258 }    
00259     
00260 legStruct LegController::process(string randomstring) {
00261     //this is the struct that is loaded from the config variables
00262     
00263     legStruct loadStruct; //local struct
00264     static int callcount=0;
00265     /* CONVERT STRING TO CHAR ARRAY */
00266     const char *cstr = randomstring.c_str();
00267     char bux2[256];
00268     /* CONVERT STRING TO CHAR ARRAY */
00269     
00270     /* leg position DIVing */
00271     //this can only be in the first position
00272     
00273     callcount++;
00274     if ((signed int) randomstring.find("leg") != -1) {
00275         loadStruct.title = "leg";
00276         loadStruct.state = LEG_POSITION_DIVE;      //NEW: separate state handles multiple dives
00277         sprintf(bux2, "\n\r process leg file: found leg label, setting state to LEG_POSITION_DIVE");
00278                 mbedLogger().appendDiagFile(bux2,3);
00279     }
00280     /* LPD */
00281     
00282     /* start-swim */
00283     if ((signed int) randomstring.find("start_swim") != -1) {
00284         loadStruct.title = "start_swim";
00285         xbee().printf("\n\rLOAD start-swim %d", randomstring.find("start_swim"));
00286         loadStruct.state = START_SWIM;
00287         sprintf(bux2, "\n\r process leg file: found START-SWIM  label, setting state to same\n");
00288                 mbedLogger().appendDiagFile(bux2,3);
00289     }
00290     /* start-swim */
00291     /* flying_idle */
00292     if ((signed int) randomstring.find("flying_idle") != -1) {
00293         loadStruct.title = "flying_idle";
00294         xbee().printf("\n\rLOAD flying-idle found at %d", randomstring.find("flying_idle"));
00295         loadStruct.state = FLYING_IDLE;
00296         sprintf(bux2, "\n\r process leg file: found FLYING_IDLE  label, setting state to FLYING_IDLE \n");
00297                 mbedLogger().appendDiagFile(bux2,3);
00298     }
00299     /* flying_idle */
00300     /* EXIT */
00301     if ((signed int) randomstring.find("exit") != -1) {
00302         loadStruct.title = "exit";
00303         xbee().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r");
00304         loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17)
00305                 sprintf(bux2, "\n\r process(valuestring) legfile: found exit key.  Callcount=%d\n", callcount);
00306                 mbedLogger().appendDiagFile(bux2,3);
00307     }
00308     /* EXIT */
00309     
00310     /*  max DEPTH TO  cycle to */
00311  
00312     if ((signed int) randomstring.find("max_depth") != -1) {
00313 
00314         int depth_pos = randomstring.find("max_depth") + 10;     //11 in example literally "depth="
00315         char depth_array[256] = {0};    //clear memory
00316         int depth_counter = 0;
00317         for (int i = depth_pos; i < randomstring.length(); i++) {
00318             if (cstr[i] == ',')
00319                 break;
00320             else if (cstr[i] == ';')
00321                 break;
00322             else {
00323                 depth_array[depth_counter] = cstr[i];
00324                 depth_counter++;
00325             }
00326         }
00327         loadStruct.max_depth = atof(depth_array);
00328         sprintf(bux2, "\n\r process legfile: key=max_depth val=%g  process(legstring)count=%d \n", atof(depth_array), callcount);
00329         mbedLogger().appendDiagFile(bux2,3);
00330 
00331     }
00332     
00333  
00334     if ((signed int) randomstring.find("min_depth") != -1) {
00335 
00336         int depth_pos = randomstring.find("min_depth") + 10;     //11 in example literally "depth="
00337         char depth_array[256] = {0};    //clear memory
00338         int depth_counter = 0;
00339         for (int i = depth_pos; i < randomstring.length(); i++) {
00340             if (cstr[i] == ',')
00341                 break;
00342             else if (cstr[i] == ';')
00343                 break;
00344             else {
00345                 depth_array[depth_counter] = cstr[i];
00346                 depth_counter++;
00347             }
00348         }
00349         loadStruct.min_depth = atof(depth_array);
00350         sprintf(bux2, "\n\r process legfile: key=min_depth val=%g \n", atof(depth_array));
00351         mbedLogger().appendDiagFile(bux2,3);
00352 
00353     }
00354     if ((signed int) randomstring.find("heading") != -1) {
00355         
00356             int depth_pos = randomstring.find("heading") + 8;     //11 in example literally "depth="
00357             char depth_array[256] = {0};    //clear memory
00358             int depth_counter = 0;
00359             for (int i = depth_pos; i < randomstring.length(); i++) {
00360                 if (cstr[i] == ',')
00361                     break;
00362                 else if (cstr[i] == ';')
00363                     break;
00364                 else {
00365                     depth_array[depth_counter] = cstr[i];
00366                     depth_counter++;
00367                 }
00368             }
00369             loadStruct.heading = atof(depth_array);
00370             sprintf(bux2, "\n\r process legfile: key=heading val=%g \n", atof(depth_array));
00371             mbedLogger().appendDiagFile(bux2,3);
00372         
00373     }
00374     /* DEPTH TO FLOAT */
00375     
00376     
00377     
00378     /* PAUSE */
00379     if ((signed int) randomstring.find("pause") != -1) {
00380         loadStruct.title = "pause";
00381     }
00382     /* PAUSE */
00383     
00384     /* TIME TO maintain leg yo-yo operations */
00385     if ((signed int) randomstring.find("timeout") != -1) { 
00386            
00387         int time_pos = randomstring.find("timeout") + 8;    //position of timeout + "timeout=" so 8
00388         char time_array[256] = {0};
00389         int time_counter = 0;
00390         for (int i = time_pos; i < randomstring.length(); i++) {
00391             //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
00392             
00393             if (cstr[i] == ',') 
00394                 break;
00395             else if (cstr[i] == ';') 
00396                 break;
00397             else {
00398                 //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
00399                 time_array[time_counter] = cstr[i]; 
00400                 time_counter++;
00401             }
00402         }
00403         loadStruct.timeout = atof(time_array);
00404         sprintf(bux2, "\n\r process legfile: key=timeout val=%g \n", atof(time_array));
00405                 mbedLogger().appendDiagFile(bux2,3);
00406     }
00407     // yo_time is to avoid a single down/or up segment going on too long - maybe 10 minutes?
00408     if ((signed int) randomstring.find("yo_time") != -1) { 
00409            
00410         int time_pos = randomstring.find("yo_time") + 8;    //position of timeout + "timeout=" so 8
00411         char time_array[256] = {0};
00412         int time_counter = 0;
00413         for (int i = time_pos; i < randomstring.length(); i++) {
00414             //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
00415             
00416             if (cstr[i] == ',') 
00417                 break;
00418             else if (cstr[i] == ';') 
00419                 break;
00420             else {
00421                 //xbee().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
00422                 time_array[time_counter] = cstr[i]; 
00423                 time_counter++;
00424             }
00425         }
00426         loadStruct.yo_time = atof(time_array);
00427         sprintf(bux2, "\n\r process legfile: key=yo_time val=%g \n", atof(time_array));
00428                 mbedLogger().appendDiagFile(bux2,3);
00429     }
00430     /* TIME TO FLOAT */  
00431     
00432 //    /* EXIT */
00433 //    if (randomstring.find("exit") != 0) {
00434 //        loadStruct.title = "exit";
00435 //        xbee().printf("\n\rEXIT.");
00436 //    }
00437 //    /* EXIT */
00438     
00439     return loadStruct;  //each iteration this returns a completed struct
00440 }