dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

Files at this revision

API Documentation at this revision

Comitter:
bouaziz
Date:
Mon Feb 04 17:07:54 2019 +0000
Parent:
6:f7c38d092b97
Child:
8:6e2b5737c86c
Commit message:
polytech labyrinthe M3PI

Changed in this revision

main3.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main3.cpp	Mon Jan 21 10:22:27 2019 +0000
+++ b/main3.cpp	Mon Feb 04 17:07:54 2019 +0000
@@ -8,6 +8,8 @@
 Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
 //MSCFileSystem fs("fs"); 
 
+LocalFileSystem local("local");   
+
 Ticker tic1;
 Timer tm1;
 
@@ -25,9 +27,9 @@
 
 
 unsigned short tabsensor[5];
-#define seuil(x) (x>400 ? 1 : 0)
+#define seuil(x) (x>350 ? 1 : 0)
 unsigned char statcapt;
-unsigned char LFRstat;
+unsigned char LFRstat, LFRvect;
 
 // fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet 
 // seuls 5 bits sont utiles
@@ -39,7 +41,7 @@
     for(unsigned short ii=0;ii<5;ii++){
             stat = (stat <<1)  | seuil(tab[ii]);
     }
-    LFRstat = ((stat&0x04)==0x04) ? 0x02 : 0) | ((stat&0x03)==0x03) ? 0x01 : 0)|((stat&0x18)==0x18) ? 0x04 : 0);
+    LFRstat = (((stat&0x04)==0x04)||((stat&0x03)==0x02)||((stat&0x18)==0x08) ? 0x02 : 0) | (((stat&0x03)==0x03) ? 0x01 : 0)|(((stat&0x18)==0x18) ? 0x04 : 0);
     return stat;
 }
 
@@ -64,6 +66,7 @@
     // limit checks on motor control
          //MIN <right < MAX
         // MIN <left < MAX
+        left = (left>MAX ? MAX :(left<MIN ? MIN : left));
         right = (right>MAX ? MAX :(right<MIN ? MIN : right));
     // send command to motors
         m3pi.left_motor(left);
@@ -115,8 +118,9 @@
 
 char sensrotation=0; // 0    1 Left 2 Right
 char rotation_flag=0;
+static unsigned char automr=0;
 void automrotation(){
-    static unsigned char autom=0;
+    
     static unsigned short vtime;
     switch(automr){
         case 0 : m3pi.stop();
@@ -144,7 +148,7 @@
                     if((statcapt&1)&& (sensrotation==2)){
                            automr=3;
                     }
-                else{
+                }else{
                     if((statcapt&0x10) && (sensrotation==1)){
                            automr=3;
                     }
@@ -172,15 +176,27 @@
 unsigned char inver;
 char trotate[2] = {1,2};
 char indexrotate=0;
+
+#define MAXSAVE 4000
+unsigned char sauvetab[MAXSAVE][3];
+
+unsigned char texttab[500];
+unsigned short intercount;
+
+unsigned lvwait,lwait2;
 int main() {
+    unsigned short i=0,ii;
     static char automain=0;
-//    myleds=0xF;
- //   FILE *p= fopen("/fs/tt.txt","a+");
+    myleds=0x0;
+    FILE *p= fopen("/local/tt.txt","w");
+    if(p!=0) {
+        myleds=1;
+    }
     wait(1.);
     m3pi.sensor_auto_calibrate();
     wait(1.);
    //3pi.cls();
-    myleds=0xF;
+    
     tic1.attach(&inter1,0.01);
     tm1.reset();
   //  fprintf(p,"ecrire dans la cle USB\r\n");
@@ -192,28 +208,177 @@
                    // myleds=0;
                     flag10ms=0;
                     statcapt=lecture_captsol(tabsensor);
+                    LFRvect = (LFRvect <<4) + LFRstat;
+                     if(i<MAXSAVE){
+                            sauvetab[i][0]=automain;
+                            sauvetab[i][1]=LFRvect;
+                            sauvetab[i][2]=statcapt;
+                            i++;
+                    }
                     switch(automain){
-                        case 0 : if(LFRstat !=0){
-                                      PIDf();  
-                                 }else{
+                        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=1;
-                                      sensrotation=trotate[indexrotate];
-                                      indexrotate = (indexrotate+1)&0x01;  //(indexrotate+1)%2;
-                                 }
+                                      automain=0;
+                                }
                                 break;
-                        case 1 :
-                                automrotation();
-                                if(rotation_flag==1){
-                                        rotation_flag=0;
-                                        automain=0;
+                        case 37 :
+                                PIDf(); 
+                                lvwait++;
+                                if(LFRvect==0x77){
+                                        if((lvwait>15)){
+                                            m3pi.stop();
+                                            automain=38;
+                                        }
+                                }else{
+                                        texttab[intercount++]='R';
+                                        automain= 13;
+                                        lvwait=0;    
                                 }
-                                break;        
+                                break;
+                        case 38 :
+                                myleds=0x09;
                         default :
                                 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) {
+                    m3pi.stop();
+                    texttab[intercount++]=0;
+                    fprintf(p,"%s\n",texttab);
+                    for(i=0;i<MAXSAVE;i++){
+                        fprintf(p,"%3u %02x %02x\n",(unsigned)sauvetab[i][0],(unsigned)sauvetab[i][1],(unsigned)sauvetab[i][2]);
+                    }
+                    myleds=0x0f;
+                    fclose(p);
+                    while(1);
+             }
+             
+                    
     }
     
 }