Diff: main.cpp

Revision:

2:48f7e5f79caa

Parent:

1:99f469d63b9a

--- a/main.cpp	Sat Jun 10 07:20:21 2017 +0000
+++ b/main.cpp	Thu Jun 22 09:32:24 2017 +0000
@@ -8,17 +8,25 @@
 Serial pc (USBTX,USBRX);
 int main()
 {
+    float infra;
     init();
     Pixy pixy= Pixy(p28,p27);
     pixy.setSerialOutput(&pc);
     pc.printf("alive");
 
     while(1) {
+        infra=1/(((SD_2.read()*3.3)-0.12766)/11.489);//valeur en cm
+        if(infra<0||infra>25) {
+            infra=25.0;
+        }
         lecture_blanc();    // recupere valeurs capteurs ligne blanche
         lecture_us();   // recupere valeurs capteurs us
         lecture_boussole(); //gBoussole
         bout=bp.read();
         blocks = pixy.getBlocks();
+        //pc.printf("infra : %f\n\r",infra);
+        //pc.printf("us gauche= %f us droit= %f\n\r",us_gauche,us_droit);
+        //wait(0.5);
         //pc.printf("etat=%d\n\r",etat);
         //pc.printf("etat=%d,capt1= %d, capt2= %d, us_ar=%f, bp=%d, boussole=%f\n\r",etat,captL1,captL3,us_arriere,bp.read(),gBoussole);
         if (blocks) {
@@ -27,145 +35,165 @@
             //pc.printf("taille=%f,sig: %d x: %d y: %d width: %d height: %d\n\r",taille, pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
             //pc.printf("OK");
         }
-        
-        switch(etat)
-        {
+
+        switch(etat) {
             case(0) :
-            
-                if(bout==0)
-                {
+
+                if(bout==0) {
                     etat=100;
                     cap_set=gBoussole;
                 }
-                if((captL1>=1)||(captL3>=1))
-                {
+                if((captL1>=1)||(captL3>=1)) {
                     etat=3;
                 }
                 break;
-            
+
             case(100):
-            
-                if((blocks)&&(taille>50))
-                {
+
+                if((blocks)&&(taille>50)) {
                     etat=1;
                 }
-                
-                else
-                {
+
+                else {
                     etat=21;
                 }
-                if((captL1>=1)||(captL3>=1))
-                {
+                if((captL1>=1)||(captL3>=1)) {
                     etat=3;
                 }
                 break;
-                 
-            case(1) :
-            
-                if(!blocks)
-                {
-                    flag++;
+            case(200):
+              if((blocks)&&(taille>50)) {
+                    etat=1;
+                }
+
+                else {
+                    etat=21;
+                }
+                if((captL1>=1)||(captL3>=1)) {
+                    etat=3;
                 }
-                if(flag>40)
-                {
+                break;
+
+            case(1) :
+
+                if(infra<20) {
+
+
                     etat=2;
-                    tempo.stop();
-                    tempo.reset();
-                    tempo.start();
-                    flag=0;
                 }
-                
-                if((captL1>=1)||(captL3>=1))
-                {
+
+                if((captL1>=1)||(captL3>=1)) {
                     etat=3;
                     flag=0;
                 }
                 break;
             case(2) :
-            
-                if(tempo.read()>0.5)
-                {
+
+                if(infra<5) {
                     etat=3;
                 }
-                if((captL1>=1)||(captL3>=1))
-                {
+                if(us_gauche<15 || us_droit<15){
+                    etat=200;
+                    }
+                if((captL1>=1)||(captL3>=1)) {
                     etat=3;
                 }
                 break;
             case(3) :
-            
-                if(us_arriere<25)
-                {
+
+                if(us_arriere<25) {
                     etat=100;
                 }
                 break;
             case(21) :
                 cap_rot=cap_set+30;
-                if(cap_rot>360)
-                {
+                if(cap_rot>360) {
                     cap_rot=cap_rot-360;
                 }
-                
-                if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180))
-                {
+
+                if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180)) {
                     etat=22;
                 }
-                if(blocks)
-                {
+                if(blocks) {
                     etat=1;
                 }
                 break;
             case(22) :
                 cap_rot=cap_set-30;
-                if(cap_rot<0)
-                {
+                if(cap_rot<0) {
                     cap_rot=cap_rot+360;
                 }
-                
-                if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180))
-                {
+
+                if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180)) {
                     etat=21;
                 }
-                if(blocks)
-                {
+                if(blocks) {
                     etat=1;
                 }
                 break;
+
+        }
+
+        switch(etat) {
+            case(0) :
+                vitmoteur(0,0);
+                break;
+
+            case(1) :
+                vitmoteur(V_max-K*errorX,V_max+K*errorX);
+                break;
+
+            case(2) :
+                cap_avant=cap_set;
+                erreur_av=cap_avant-gBoussole;
+                
+                if(erreur_av>20) {
+                    erreur_av=20;
+                }
+                if(erreur_av<-20) {
+                    erreur_av=-20;
+                }
+                vitmoteur(V_max*1.5+erreur_av*0.2,V_max*1.5-erreur_av*0.2);
+                //vitmoteur(V_max*1.5,V_max*1.5);
+                break;
+
+            case(3) :
+
+                cap_arriere=cap_set;
+                erreur_ar=cap_arriere-gBoussole;
+
+                if(erreur_ar>20) {
+                    erreur_ar=20;
+                }
+                if(erreur_ar<-20) {
+                    erreur_ar=-20;
+                }
+                vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2);
+
+                break;
+
+            case(21) :
+                vitmoteur(V_recherche,-V_recherche);
+                break;
+
+            case(22) :
+                vitmoteur(-V_recherche,V_recherche);
+                break;
+
+            case(100) :
+                break;
+            
+            case(200):
+            vitmoteur(0,20);
+            wait(2);
+            vitmoteur(20,20);
+            wait(2);
+            break;
             
         }
-        
-        switch(etat)
-        {
-            case(0) : vitmoteur(0,0);break;
-            
-            case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
-            
-            case(2) : vitmoteur(V_max*1.5,V_max*1.5);break;
-            
-            case(3) : 
-            
-                    cap_arriere=cap_set;
-                    erreur_ar=cap_arriere-gBoussole;
-                    
-                    if(erreur_ar>20)
-                    {
-                        erreur_ar=20;
-                    }
-                    if(erreur_ar<-20)
-                    {
-                        erreur_ar=-20;
-                    }
-                    vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2);break;
-            
-            case(21) : vitmoteur(V_recherche,-V_recherche);break;           
-            
-            case(22) : vitmoteur(-V_recherche,V_recherche);break;
-            
-            case(100) : break;
-        } 
-       
+
     }
 }
-        
+
 
-            
+