,,,,,,,,,,,,

Dependencies:   CMPS03 SRF05 mbed pixy

Fork of 0000Non_stop_code_v1 by Alexandre Pirotte

Files at this revision

API Documentation at this revision

Comitter:
pirottealex
Date:
Sat Jun 10 07:20:21 2017 +0000
Parent:
0:a8cee96c9250
Commit message:
llllllllllllllllllllllllllll;

Changed in this revision

fct.h Show annotated file Show diff for this revision Revisions of this file
globals.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r a8cee96c9250 -r 99f469d63b9a fct.h
--- 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;
 
diff -r a8cee96c9250 -r 99f469d63b9a globals.cpp
--- 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
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;