Diff: main.cpp

Revision:

1:99f469d63b9a

Parent:

0:a8cee96c9250

diff -r a8cee96c9250 -r 99f469d63b9a main.cpp
--- a/main.cpp	Sat Jun 10 04:58:21 2017 +0000
+++ b/main.cpp	Sat Jun 10 07:20:21 2017 +0000
@@ -24,10 +24,10 @@
         if (blocks) {
             taille=pixy.blocks[j].width*pixy.blocks[j].height;
             errorX = (160-pixy.blocks[j].x);
-            //pc.printf("sig: %d x: %d y: %d width: %d height: %d\n\r", pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height);
+            //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)
         {
             case(0) :
@@ -37,11 +37,15 @@
                     etat=100;
                     cap_set=gBoussole;
                 }
+                if((captL1>=1)||(captL3>=1))
+                {
+                    etat=3;
+                }
                 break;
             
             case(100):
             
-                if(blocks)
+                if((blocks)&&(taille>50))
                 {
                     etat=1;
                 }
@@ -50,16 +54,31 @@
                 {
                     etat=21;
                 }
+                if((captL1>=1)||(captL3>=1))
+                {
+                    etat=3;
+                }
                 break;
                  
             case(1) :
             
                 if(!blocks)
                 {
+                    flag++;
+                }
+                if(flag>40)
+                {
                     etat=2;
                     tempo.stop();
                     tempo.reset();
                     tempo.start();
+                    flag=0;
+                }
+                
+                if((captL1>=1)||(captL3>=1))
+                {
+                    etat=3;
+                    flag=0;
                 }
                 break;
             case(2) :
@@ -68,6 +87,10 @@
                 {
                     etat=3;
                 }
+                if((captL1>=1)||(captL3>=1))
+                {
+                    etat=3;
+                }
                 break;
             case(3) :
             
@@ -117,9 +140,22 @@
             
             case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break;
             
-            case(2) : vitmoteur(V_max,V_max);break;
+            case(2) : vitmoteur(V_max*1.5,V_max*1.5);break;
+            
+            case(3) : 
             
-            case(3) : vitmoteur(-V_moy,-V_moy);break;
+                    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;