Revision 1:99f469d63b9a, committed 2017-06-10

Comitter:

pirottealex

Date:

Sat Jun 10 07:20:21 2017 +0000

Parent:

0:a8cee96c9250

Commit message:

llllllllllllllllllllllllllll;

Changed in this revision

--- a/fct.h	Sat Jun 10 04:58:21 2017 +0000
+++ b/fct.h	Sat Jun 10 07:20:21 2017 +0000
@@ -7,11 +7,11 @@
 extern PwmOut MotG;
 extern int captL1;
 extern int captL3;
-extern float us_arriere,gBoussole,cap_set;
+extern float us_arriere,gBoussole,cap_set,erreur_ar,flag;
 extern int j,etat,bout;
 extern uint16_t blocks;
-extern float taille,errorX,cap_rot;
-extern Timer tempo;
+extern float taille,errorX,cap_rot,cap_arriere;
+extern Timer tempo,tempo2;
 //GLOBALES
 extern BusOut leds;
 

--- a/globals.cpp	Sat Jun 10 04:58:21 2017 +0000
+++ b/globals.cpp	Sat Jun 10 07:20:21 2017 +0000
@@ -27,6 +27,8 @@
 InterruptIn I_D(p30);
 InterruptIn I_G(p29);
 
+Timer tempo2;
+
 SPI spi(p5,p6,p7);
 
 AnalogIn C1(p15);// capteur de ligne blanche 1
@@ -40,7 +42,7 @@
 int j=0,etat=0;
 uint16_t blocks;
 float taille,errorX,gBoussole,cap_set,cap_rot;
-int captL1,bout;
+int captL1,bout,flag;
 int captL3;
-float us_arriere;
+float us_arriere,cap_arriere,erreur_ar;
 Timer tempo;
\ No newline at end of file

--- 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;