joey QUANTIN
pixy info2
Dependencies: CMPS03 SRF05 mbed pixy
Fork of
0000Non_stop_code_v3
by 
Alexandre Pirotte
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
--- 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)
--- 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
--- 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
--- 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;
- }
-
+
}
}
-
+
-
+