![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
pixy info2
Dependencies: CMPS03 SRF05 mbed pixy
Fork of 0000Non_stop_code_v3 by
Revision 2:48f7e5f79caa, committed 2017-06-22
- Comitter:
- qj604184
- Date:
- Thu Jun 22 09:32:24 2017 +0000
- Parent:
- 1:99f469d63b9a
- Commit message:
- pixy
Changed in this revision
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; - } - + } } - + - +