dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

Revision:
8:6e2b5737c86c
Parent:
7:a7bf354ae965
--- a/main3.cpp	Mon Feb 04 17:07:54 2019 +0000
+++ b/main3.cpp	Mon Feb 11 09:46:33 2019 +0000
@@ -180,13 +180,301 @@
 #define MAXSAVE 4000
 unsigned char sauvetab[MAXSAVE][3];
 
-unsigned char texttab[500];
-unsigned short intercount;
+unsigned char texttab[300];
+unsigned char TXT[20]={"FLFLLRLLRR"};
+unsigned short intercount,index;
 
 unsigned lvwait,lwait2;
+char mess[20];
+
+char rejoue(){
+     static unsigned char automain2=0;
+      switch(automain2){
+                case 0 : PIDf();
+                     switch(LFRvect) {
+                          case 0x77 :
+                                    automain2=37;
+                                    lvwait=0;
+                                    break;
+                          case 0x00 : // pas bon
+                                    m3pi.stop();
+                                    m3pi.cls();
+                                    sprintf(mess,"ERR0:%u",index);
+                                    m3pi.print(mess,strlen(mess));
+                                    while(1);
+                                    //Indiquer probleme simplification
+                                    break;
+                          case 0x33 :
+                                    automain2= 13;
+                                    
+                                    break;
+                          case 0x66 :
+                                    automain2= 13;
+                                    
+                                    break;
+                          case 0x22 :
+                                     
+                                   // break;
+                          default :
+                                    break;
+                        }          
+                        break;
+                case 2 : // fin rotation droite
+                        if(LFRvect==0x22){
+                                m3pi.stop();
+                                automain2=0;
+                        }
+                        break; 
+                case 13 : //inter droite ou tout droit
+                        PIDf(); 
+                        if((LFRvect ==0x00) || (LFRvect ==0x22)) {
+                                switch(TXT[index]){
+                                        case 'F' :
+                                                    index++;
+                                                    automain2 =0;
+                                                    break;
+                                        case 'R' :
+                                                    //m3pi.stop();
+                                                    lvwait=0;
+                                                    index++;
+                                                    if(LFRvect == 0x00){
+                                                              m3pi.right(0.2);
+                                                              automain2=2;
+                                                    }else{
+                                                                // alors c'est 0x22
+                                                               automain2 =14;  //on tourne à droite                                                                                    }
+                                                    break;
+                                        case 'L' :
+                                                    lvwait=0;
+                                                    index++;
+                                                    if(LFRvect == 0x00){
+                                                              m3pi.left(0.2);
+                                                              automain2=2;
+                                                    }else{
+                                                               automain2 =17;  //on tourne à droite
+                                                    }                                                                                    }                                                           
+                                                    break;
+                                         default :
+                                                    break;
+                                }
+                        }
+                        break;
+                                     
+                case 14 :
+                        lvwait++;
+                        if(lvwait>(60/10)){
+                                m3pi.stop();
+                                automain=15;
+                                lvwait=0;
+                        }
+                        break;
+                       
+                case 15 :
+                        lvwait++;
+                        if(lvwait>(500/10)){
+                                m3pi.right(0.2);
+                                automain=16;
+                                lvwait=0;
+                        }
+                        break;
+               case 16 :       
+                        if((LFRvect&0x22)==0){
+                                automain=2;
+                        }
+                        break;      
+               case 17 :
+                        lvwait++;
+                        if(lvwait>(60/10)){
+                                m3pi.stop();
+                                automain2=18;
+                                lvwait=0;
+                        }
+                        break;  
+            
+               case 18 :
+                        lvwait++;
+                        if(lvwait>(500/10)){
+                                m3pi.left(0.2);
+                                automain2=16;
+                                lvwait=0;
+                        }
+                        break;                
+                case 37 :
+                        PIDf();
+                        lvwait++;
+                        if(LFRvect==0x77){
+                                if((lvwait>15)){
+                                    m3pi.stop();
+                                    automain2=38;
+                                    
+                                }
+                        }else{
+                              
+                                automain2= 13;
+                                lvwait=0;   
+                        }
+                        break;
+                case 38 :
+                        myleds=0x0F;
+                        return 1;
+                default :
+                        break;
+    }
+    return 0;
+}
+
+
+char explore(){
+     static unsigned char automain=0;
+      switch(automain){
+            case 0 : PIDf(); 
+                     switch(LFRvect) {
+                          case 0x77 :
+                                    automain=37;
+                                    lvwait=0;
+                                    break;
+                          case 0x00 :
+                                    //m3pi.stop();
+                                    automain= 3;
+                                    lvwait=0;
+                                    texttab[intercount++]='B';
+                                    break;
+                          case 0x33 :
+                                    automain= 13;
+                                    texttab[intercount++]='R';
+                                    break;
+                          case 0x66 :
+                                    automain= 26;
+                                    
+                                    break;
+                          case 0x22 :
+                                     
+                                   // break;
+                          default :
+                                    break;
+                    }          
+                    break;
+            case 3 : lvwait++;
+                    if(lvwait>(60/10)){
+                            m3pi.stop();
+                            automain=1;
+                    }
+                    break;       
+            case 1 : //lancer STOP avec rotation  à droite
+                    lvwait++;
+                    if(lvwait>(500/10)){
+                            m3pi.right(0.2);
+                            automain=2;
+                    }
+                    break;
+            case 2 : // jattends pour STOPPER
+                    if(LFRvect==0x22){
+                        m3pi.stop();
+                        automain=0;
+                    }
+                    break; 
+            case 13 : //inter droite ou tout droit 
+                    PIDf();  
+                    switch(LFRvect){
+                            case 0x00 :
+                                        m3pi.stop();
+                                        lvwait=0;
+                                        automain =1;  
+                            case 0x22 :
+                                        //m3pi.stop();
+                                        lvwait=0;
+                                        automain =14;  
+                            default :
+                                    break;
+                    }
+                    break; 
+                                  
+            case 14 : 
+                    lvwait++;
+                    if(lvwait>(60/10)){
+                            m3pi.stop();
+                            automain=15;
+                            lvwait=0;
+                    }
+                    break;
+                    
+            case 15 :
+                    lvwait++;
+                    if(lvwait>(500/10)){
+                            m3pi.right(0.2);
+                            automain=16;
+                            lvwait=0;
+                    }
+                    break;
+            case 16 :        
+                    if((LFRvect&0x22)==0){
+                            automain=2;
+                    }
+                    break;
+            case 26 : // inter  gauche ou tout droit
+                    PIDf();  
+                    switch(LFRvect){
+                            case 0x00 :
+                                       // m3pi.stop();
+                                        lvwait=0;
+                                        automain =27; 
+                                        texttab[intercount++]='L';
+                                        break; 
+                            case 0x22 : texttab[intercount++]='F';
+                                        automain =0; 
+                                        break; 
+                            default :
+                                    break;
+                    }
+                    break; 
+            case 27 :
+                    lvwait++;
+                    if(lvwait>(50/10)){
+                            m3pi.stop();
+                            automain=28;
+                            lvwait=0;
+                    } 
+                    break;      
+            case 28 :
+                    lvwait++;
+                    if(lvwait>(500/10)){
+                            m3pi.left(0.2);     
+                            automain=2;
+                    }
+                    break;
+            case 29 :
+                    if(lvwait++>2){
+                          m3pi.stop();
+                          automain=0;
+                    }
+                    break;
+            case 37 :
+                    PIDf(); 
+                    lvwait++;
+                    if(LFRvect==0x77){
+                            if((lvwait>15)){
+                                m3pi.stop();
+                                automain=38;
+                            }
+                    }else{
+                            texttab[intercount++]='R';
+                            automain= 13;
+                            lvwait=0;    
+                    }
+                    break;
+            case 38 :
+                    myleds=0x0F;
+                    return 1;
+            default :
+                    break;
+        }
+        return 0;                  
+}
+
+
 int main() {
     unsigned short i=0,ii;
-    static char automain=0;
+    unsigned char supauto=2;
     myleds=0x0;
     FILE *p= fopen("/local/tt.txt","w");
     if(p!=0) {
@@ -209,164 +497,42 @@
                     flag10ms=0;
                     statcapt=lecture_captsol(tabsensor);
                     LFRvect = (LFRvect <<4) + LFRstat;
-                     if(i<MAXSAVE){
+                  /*   if(i<MAXSAVE){
                             sauvetab[i][0]=automain;
                             sauvetab[i][1]=LFRvect;
                             sauvetab[i][2]=statcapt;
                             i++;
-                    }
-                    switch(automain){
-                        case 0 : PIDf(); 
-                                 switch(LFRvect) {
-                                      case 0x77 :
-                                                automain=37;
-                                                lvwait=0;
-                                                break;
-                                      case 0x00 :
-                                                //m3pi.stop();
-                                                automain= 3;
+                    }*/
+                    switch(supauto){
+                            case 0 :
+                                    if(explore()){
+                                                // simplification
+                                                //attendre
                                                 lvwait=0;
-                                                texttab[intercount++]='B';
-                                                break;
-                                      case 0x33 :
-                                                automain= 13;
-                                                texttab[intercount++]='R';
-                                                break;
-                                      case 0x66 :
-                                                automain= 26;
-                                                
-                                                break;
-                                      case 0x22 :
-                                                 
-                                               // break;
-                                      default :
-                                                break;
-                                }          
-                                break;
-                        case 3 : lvwait++;
-                                if(lvwait>(60/10)){
-                                        m3pi.stop();
-                                        automain=1;
-                                }
-                                break;       
-                        case 1 : //lancer STOP avec rotation  à droite
-                                lvwait++;
-                                if(lvwait>(500/10)){
-                                        m3pi.right(0.2);
-                                        automain=2;
-                                }
-                                break;
-                        case 2 : // jattends pour STOPPER
-                                if(LFRvect==0x22){
-                                    m3pi.stop();
-                                    automain=0;
-                                }
-                                break; 
-                        case 13 : //inter droite ou tout droit 
-                                PIDf();  
-                                switch(LFRvect){
-                                        case 0x00 :
-                                                    m3pi.stop();
-                                                    lvwait=0;
-                                                    automain =1;  
-                                        case 0x22 :
-                                                    //m3pi.stop();
-                                                    lvwait=0;
-                                                    automain =14;  
-                                        default :
-                                                break;
-                                }
-                                break; 
-                                              
-                        case 14 : 
-                                lvwait++;
-                                if(lvwait>(60/10)){
-                                        m3pi.stop();
-                                        automain=15;
-                                        lvwait=0;
-                                }
-                                break;
-                                
-                        case 15 :
-                                lvwait++;
-                                if(lvwait>(500/10)){
-                                        m3pi.right(0.2);
-                                        automain=16;
-                                        lvwait=0;
-                                }
-                                break;
-                        case 16 :        
-                                if((LFRvect&0x22)==0){
-                                        automain=2;
-                                }
-                                break;
-                        case 26 : // inter  gauche ou tout droit
-                                PIDf();  
-                                switch(LFRvect){
-                                        case 0x00 :
-                                                   // m3pi.stop();
-                                                    lvwait=0;
-                                                    automain =27; 
-                                                    texttab[intercount++]='L';
-                                                    break; 
-                                        case 0x22 : texttab[intercount++]='F';
-                                                    automain =0; 
-                                                    break; 
-                                        default :
-                                                break;
-                                }
-                                break; 
-                        case 27 :
-                                lvwait++;
-                                if(lvwait>(50/10)){
-                                        m3pi.stop();
-                                        automain=28;
-                                        lvwait=0;
-                                } 
-                                break;      
-                        case 28 :
-                                lvwait++;
-                                if(lvwait>(500/10)){
-                                        m3pi.left(0.2);     
-                                        automain=2;
-                                }
-                                break;
-                        case 29 :
-                                if(lvwait++>2){
-                                      m3pi.stop();
-                                      automain=0;
-                                }
-                                break;
-                        case 37 :
-                                PIDf(); 
-                                lvwait++;
-                                if(LFRvect==0x77){
-                                        if((lvwait>15)){
-                                            m3pi.stop();
-                                            automain=38;
-                                        }
-                                }else{
-                                        texttab[intercount++]='R';
-                                        automain= 13;
-                                        lvwait=0;    
-                                }
-                                break;
-                        case 38 :
-                                myleds=0x09;
-                        default :
-                                break;
+                                                supauto=1;
+                                    break;
+                            case 1 : 
+                                    lvwait++;
+                                    if(lvwait>=1500){
+                                            myleds=0;
+                                            supauto=2;
+                                    break;
+                            case 2 :
+                                    if(rejoue()){
+                                            supauto=3;
+                                    }
+                                    break;
+                            case 3 :
+                                    myleds(0x06);
+                            default :
+                             // se mettre en arret et afficher ereur
+                                    break;    
+                            
                     }
                     
-  /*                  if(i<MAXSAVE){
-                            sauvetab[i][0]=automain;
-                            sauvetab[i][1]=statcapt;
-                            sauvetab[i][2]=LFRvect;
-                            sauvetab[i][3]=automr;
-                            i=i+1;
-                    }
-    */                
+   
               }
-              if(i>=MAXSAVE) {
+ /*             if(i>=MAXSAVE) {
                     m3pi.stop();
                     texttab[intercount++]=0;
                     fprintf(p,"%s\n",texttab);
@@ -376,7 +542,7 @@
                     myleds=0x0f;
                     fclose(p);
                     while(1);
-             }
+             }*/
              
                     
     }