pixy info2

Dependencies:   CMPS03 SRF05 mbed pixy

Fork of 0000Non_stop_code_v3 by Alexandre Pirotte

Files at this revision

API Documentation at this revision

Comitter:
qj604184
Date:
Thu Jun 22 09:32:24 2017 +0000
Parent:
1:99f469d63b9a
Commit message:
pixy

Changed in this revision

fct.cpp Show annotated file Show diff for this revision Revisions of this file
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 99f469d63b9a -r 48f7e5f79caa fct.cpp
--- a/fct.cpp	Sat Jun 10 07:20:21 2017 +0000
+++ b/fct.cpp	Thu Jun 22 09:32:24 2017 +0000
@@ -22,14 +22,14 @@
 }
 void lecture_blanc(void)
 {
-    if(C1.read()>0.5) {
+    if(C1.read()>0.8) {
         captL1=0;
-    } else {
+    } else if(C1.read()<0.2) {
         captL1=1;
     }
-    if(C3.read()>0.5) {
+    if(C3.read()>0.8) {
         captL3=0;
-    } else {
+    } else if(C3.read()<0.2) {
         captL3=1;
     }
 }
@@ -37,6 +37,8 @@
 void lecture_us(void)
 {
     us_arriere=us_arr.read();
+    us_gauche=us_g.read();
+    us_droit=us_d.read();
 }
 
 void init(void)
diff -r 99f469d63b9a -r 48f7e5f79caa fct.h
--- a/fct.h	Sat Jun 10 07:20:21 2017 +0000
+++ b/fct.h	Thu Jun 22 09:32:24 2017 +0000
@@ -7,10 +7,10 @@
 extern PwmOut MotG;
 extern int captL1;
 extern int captL3;
-extern float us_arriere,gBoussole,cap_set,erreur_ar,flag;
+extern float us_arriere,us_droit,us_gauche,gBoussole,cap_set,erreur_ar,flag,erreur_av;
 extern int j,etat,bout;
 extern uint16_t blocks;
-extern float taille,errorX,cap_rot,cap_arriere;
+extern float taille,errorX,cap_rot,cap_arriere,cap_avant;
 extern Timer tempo,tempo2;
 //GLOBALES
 extern BusOut leds;
@@ -24,6 +24,8 @@
 extern DigitalOut trig3;//US3
 extern InterruptIn echo3;
 extern SRF05 us_arr;
+extern SRF05 us_g;
+extern SRF05 us_d;
 extern I2C monI2C;
 extern DigitalOut cs;
 extern PwmOut Servo;
@@ -48,7 +50,7 @@
 //CONSTANTES
 #define ADR_PCF 0x70
 #define PERIOD 0.0001
-#define V_max 30
+#define V_max 32
 #define V_recherche 18
 #define K 0.1
 #define V_moy 25
diff -r 99f469d63b9a -r 48f7e5f79caa globals.cpp
--- a/globals.cpp	Sat Jun 10 07:20:21 2017 +0000
+++ b/globals.cpp	Thu Jun 22 09:32:24 2017 +0000
@@ -15,6 +15,9 @@
 InterruptIn echo3(p23);*/
 
 SRF05 us_arr(p8,p24);
+SRF05 us_g(p11,p12);
+SRF05 us_d(p26,p23);
+
 CMPS03 Boussole(p9,p10,0xC0);
 I2C monI2C(p9,p10);
 PwmOut Servo(p21);
@@ -44,5 +47,5 @@
 float taille,errorX,gBoussole,cap_set,cap_rot;
 int captL1,bout,flag;
 int captL3;
-float us_arriere,cap_arriere,erreur_ar;
+float us_arriere,us_gauche,us_droit,cap_arriere,erreur_ar,erreur_av,cap_avant;
 Timer tempo;
\ No newline at end of file
diff -r 99f469d63b9a -r 48f7e5f79caa main.cpp
--- 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;
-        } 
-       
+
     }
 }
-        
+
 
-            
+