Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
Diff: main.cpp
- Revision:
- 7:2f4660e9cf92
- Parent:
- 6:5f7df5c74a77
--- a/main.cpp Wed May 31 08:48:23 2017 +0000 +++ b/main.cpp Fri Jun 23 11:19:58 2017 +0000 @@ -1,60 +1,245 @@ #include "mbed.h" #include "fct.h" -BusOut ledsetat(p12,p13); +#include "Pixy.h" +#include "PixyLink.h" +#include "CMPS03.h" + + +Serial pc(USBTX,USBRX); +CMPS03 compass(p9, p10, CMPS03_DEFAULT_I2C_ADDRESS); int main() { - int etat=0; + init(); - while(1) { - // printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG); - //wait(0.05); - ledsetat.write(etat); + Pixy pixy = Pixy(p28,p27); + pixy.setSerialOutput(&pc); + int tempo=0; + int i=0; + while(true) { + //capg=capb1.read(); + //capd=capb2.read(); + capg=0; + capd=0; + + //i++;pc.printf("%d\n\r",i); + jck=jack.read(); + leds.write(etat); + //pc.printf("g=%d,d=%d\n\r",capd,capg); + //pc.printf("etat=%d,taille=%f,capt_eg=%3.0f,capt_g=%3.0f,capt_m=%3.0f,capt_d=%3.0f,capt_ed=%3.0f\n\r",etat,taille,capt_eg,capt_g,capt_m,capt_d,capt_ed); + //pc.printf("etat=%d, cap_lum=%f, cap=%f, block=%d,taille=%f\n\r",etat,cap_lum,cap,blocks,taille); + cap=compass.readBearing()/10.0; + blocks = pixy.getBlocks(); + if (blocks) { + taille=pixy.blocks[j].width*pixy.blocks[j].height; + errorX = (160-pixy.blocks[j].x); + errorY = (100-pixy.blocks[j].y); + } /*else if(!blocks) { + blocks = pixy.getBlocks(); + taille=pixy.blocks[j].width*pixy.blocks[j].height; + errorX = (160-pixy.blocks[j].x); + errorY = (100-pixy.blocks[j].y); + }*/ + if(tr.read()>10) { + tempo=1; + } + + //pc.printf("taille=%f\n\r",taille); switch(etat) { - case 0 : - if((AN2>10 && AN2<20) || US3<13) { + case(0) : + if(jck==1) { etat=1; - stopMotor(); + cap_lum=cap; + tr.stop(); + tr.reset(); + tr.start(); + } + break; + + case(1) : + + cap_lum=cap; + if(!blocks) { + etat=3; + } + if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) { + etat=2; } - if(US2>100 && US1<40) { - etat=2; - stopMotor(); + + + if((taille>seuil_taille)&&(tempo==1)) { + tb.reset(); + tb.start(); + etat=100; + } + break; + + case(2) : + if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(blocks)) { + etat=1; + } + if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(!blocks)) { + + etat=3; + } + + if((taille>seuil_taille)&&(tempo==1)) { + tb.reset(); + tb.start(); + etat=100; } break; - case 1 : - if(US2>100 && US1<40) { + + + case(3) : + if(cap_lum<180) { + if(cap<=cap_lum+180 && cap>cap_lum) { + etat=5; + } else { + etat=4; + } + } else { + if(cap<=cap_lum && cap>cap_lum-180) { + etat=4; + } + + else { + etat=5; + } + } + + if((taille>seuil_taille)&&(tempo==1)) { + tb.reset(); + tb.start(); + etat=100; + } + break; + + case(4) : //pc.printf("tourner a gauche"); + + if (blocks) { + etat=1; + } + if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) { etat=2; - stopMotor(); + } + + if((taille>seuil_taille)&&(tempo==1)) { + tb.reset(); + tb.start(); + etat=100; } - if(AN2>30) { - etat=0; - t2.start(); - t2.reset(); - stopMotor(); + break; + + case(5) : + + //pc.printf("touner a droite"); + if (blocks) { + etat=1; + } + if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) { + etat=2; + } + + if((taille>seuil_taille)&&(tempo==1)) { + tb.reset(); + tb.start(); + etat=100; + } + break; + + case(10) : + + if(capt_m>=40) { + etat=2; + } + break; + + + case(100) : + + if((capt_m<30) || (capt_g<25)) { + etat=101; + t.stop(); + t.reset(); + t.start(); + } + break; + + case(101) : + if(t.read()<10) { + servo_start(); + } else if(t.read()>=10) { + servo_stop(); } break; - case 2 : - if((AN2>10 && AN2<20) || US3<13) { - etat=1; - stopMotor(); - } - if(t2.read()>2.5) { - etat=0; - } - break; + + } switch(etat) { - case 0 : - suivi_mur(); + case(0) : + vitesse(0,0); + break; + + case(1) : + vitesse(V_max-K*errorX,V_max+K*errorX); break; - case 1 : - rotation_horaire(); + case(2) : + erreur_cap=(cap_lum+(80-(capt_eg+capt_g+capt_m)/3)*Kdist-(80-(capt_ed+capt_d)/2)*Kdist)-cap; + if(erreur_cap>180) { + erreur_cap=erreur_cap-360; + } + if(erreur_cap<-180) { + erreur_cap=erreur_cap+360; + } + if(erreur_cap>0) { + cmdD=V_max-Kcap*erreur_cap; + cmdG=V_max; + } else { + cmdD=V_max; + cmdG=V_max+Kcap*erreur_cap; + } + if(cmdD<0) { + cmdD=0; + } + if(cmdG<0) { + cmdG=0; + } + vitesse(cmdG,cmdD); + break; + case(3) : break; - case 2 : - contournement(); + + case(4) : + vitesse(V_moy,lent); + break; + + case(5) : + vitesse(lent,V_moy); + break; + + case(10): + vitesse(lent,lent); break; + + case(100) : + + vitesse(V_max-K*errorX,V_max+K*errorX); + break; + + case(101) : + + vitesse(0,0); + break; + case(111) : + vitesse(-30,0); + break; + + case(112) : + vitesse(0,-30); + break; + } - } }