tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
Diff: main.cpp
- Revision:
- 9:7f62c0329849
- Parent:
- 7:64bfecbb3000
- Child:
- 10:1a69a6f659bd
diff -r 64bfecbb3000 -r 7f62c0329849 main.cpp --- a/main.cpp Wed May 31 12:17:59 2017 +0000 +++ b/main.cpp Wed May 31 16:30:10 2017 +0000 @@ -8,23 +8,19 @@ { int etat=0; init(); + t3.reset(); + t3.start(); 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("set=%.1f CAP=%.1f\n\r",bearing_set,bearing); + //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing); //wait(0.1); bearing=boussole.readBearing()/10.0; - t3.reset(); - t3.start(); + ledsetat.write(etat); switch(etat) { case 0 : - if(t3.read()>4) { - etat=1; - t3.reset(); - stop(); - } if((AN2>10 && AN2<20) || US3<13) { etat=1; stop(); @@ -33,8 +29,16 @@ etat=2; stop(); } + if(t3.read()>10) { + etat=3; + bearing_set=bearing_set+25.0; + t3.reset(); + t3.stop(); + stop(); + } break; case 1 : + if(US2>100 && US1<40) { etat=2; stop(); @@ -45,10 +49,6 @@ t2.reset(); stop(); } - if(bearing>(bearing-90.0)) { - etat=0; - stop(); - } break; case 2 : if((AN2>10 && AN2<20) || US3<13) { @@ -59,6 +59,19 @@ etat=0; } break; + case 3 : + if(bearing>bearing_set) { + etat=4; + t3.reset(); + stop(); + } + break; + case 4 : + if((AN2>10 && AN2<22) || US3<22) { + etat=1; + stop(); + } + break; } switch(etat) { @@ -71,6 +84,12 @@ case 2 : contournement(); break; + case 3 : + rotation_horaire(); + break; + case 4 : + en_avant(); + break; } }