tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
Diff: main.cpp
- Revision:
- 13:4fdd8ad91c69
- Parent:
- 12:6151eb503170
diff -r 6151eb503170 -r 4fdd8ad91c69 main.cpp --- a/main.cpp Thu Jun 01 09:30:37 2017 +0000 +++ b/main.cpp Mon Jan 22 19:32:00 2018 +0000 @@ -1,9 +1,5 @@ - #include "fct.h" - -BusOut ledsetat(p12,p13); - int main() { @@ -11,48 +7,48 @@ int etat=0; init(); t3.reset(); + t3.stop(); 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); - //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing); + //printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f jack=%d\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG,jack.read()); //wait(0.1); - - bearing=boussole.readBearing()/10.0; - ledsetat.write(etat); - - if(bearing<bearing_set+10.0 && bearing>bearing_set-10.0) { - t3.start(); - } else { - t3.stop(); - } + //printf("etat=%d set=%.1f CAP=%.1f t3=%f capt1=%d capt2=%d\n\r",etat,bearing_set,bearing,t3.read(),capt1.read(),capt2.read()); + //wait(0.1); + capt=capt2.read(); switch(etat) { case 0 : - if(jack.read()==0) { + if(jack.read()==1) { etat=1; t3.reset(); + t3.start(); + t5.reset(); + t5.start(); } break; case 1 : - if(jack.read()!=0) etat=0; - if((AN2>40 && AN2<45) || US3<40) { + + if(jack.read()!=1) etat=0; + if((AN2>10 && AN2<40) || US3<18) { etat=2; stop(); } - if(US2>100 && AN1<40) { + if(US2>100 && US1<40) { etat=3; stop(); } - if(t3.read()>8) { + /*if(t3.read()>7) { etat=4; - bearing_set=bearing_set+25.0; - t3.reset(); - t3.stop(); + stop(); + }*/ + if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { + etat=9; + t4.reset(); + t4.start(); stop(); } break; case 2 : - if(jack.read()!=0) etat=0; + if(jack.read()!=1) etat=0; if(US2>100 && US1<40) { etat=3; stop(); @@ -63,33 +59,108 @@ t2.reset(); stop(); } + if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { + etat=9; + t4.reset(); + t4.start(); + stop(); + } break; case 3 : - if(jack.read()!=0) etat=0; - if((AN2>40 && AN2<45) || US3<40) { + if(jack.read()!=1) etat=0; + if((AN2>10 && AN2<30) || US3<18) { etat=2; stop(); } - if(t2.read()>2) { + if(t2.read()>5) { etat=1; stop(); } + if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { + etat=9; + t4.reset(); + t4.start(); + stop(); + } break; case 4 : - if(jack.read()!=0) etat=0; - if(bearing>bearing_set) { - etat=5; - t3.reset(); + etat=5; + t3.reset(); + stop(); + + break; + case 5 : + if(US3<40 || US2<50) { + etat=6; stop(); } break; - case 5 : - if(jack.read()!=0) etat=0; - if((AN2>40 && AN2<42) || US3<40) { - etat=2; + case 6 : + t3.start(); + if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { + etat=9; + t4.reset(); + t4.start(); + stop(); + } + if(jack.read()!=1) etat=0; + if((AN2>10 && AN2<30) || US3<18) { + etat=7; + stop(); + } + if(US2>100 && US1<40) { + etat=8; + stop(); + } + if(t3.read()>58) { + etat=9; + } + break; + case 7 : + if(jack.read()!=1) etat=0; + if(US2>100 && US1<40) { + etat=8; + stop(); + } + if(AN2>30) { + etat=6; + t2.start(); + t2.reset(); + stop(); + } + if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { + etat=9; + t4.reset(); + t4.start(); stop(); } break; + case 8 : + if(jack.read()!=1) etat=0; + if((AN2>10 && AN2<30) || US3<18) { + etat=7; + stop(); + } + if(t2.read()>4) { + etat=6; + stop(); + } + if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) { + etat=9; + t4.reset(); + t4.start(); + stop(); + } + break; + case 9 : + if(t4.read()>2) etat=10; + stop(); + break; + case 10 : + if(jack.read()!=1) etat=0; + stop(); + break; + } switch(etat) { @@ -106,11 +177,31 @@ contournement(); break; case 4 : - rotation_sharp(); + rotation_horaire(); + wait_ms(700); break; case 5 : en_avant(); break; + case 6 : + suivi_mur_dist(); + break; + case 7 : + rotation_sharp(); + break; + case 8 : + contournement(); + break; + case 9 : + ballon(); + smoke.write(1); + break; + case 10 : + stop(); + if(t4.read()>3) { + smoke.write(0); + } + break; } }