tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
Diff: main.cpp
- Revision:
- 10:1a69a6f659bd
- Parent:
- 9:7f62c0329849
- Child:
- 11:cb477f6ec781
--- a/main.cpp Wed May 31 16:30:10 2017 +0000 +++ b/main.cpp Thu Jun 01 07:54:55 2017 +0000 @@ -13,14 +13,22 @@ 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 set=%.1f CAP=%.1f jack=%d\n\r",etat,bearing_set,bearing,jack.read()); //wait(0.1); - + bearing=boussole.readBearing()/10.0; + ledsetat.write(etat); - ledsetat.write(etat); + if(bearing<bearing_set+10.0 && bearing>bearing_set-10.0) { + t3.start(); + }else { + t3.stop(); + } + switch(etat) { - case 0 : + case 0 : if(jack.read()==1) etat=1; break; + + case 1 : if((AN2>10 && AN2<20) || US3<13) { etat=1; stop(); @@ -30,15 +38,14 @@ stop(); } if(t3.read()>10) { - etat=3; + etat=3; bearing_set=bearing_set+25.0; t3.reset(); t3.stop(); stop(); } break; - case 1 : - + case 2 : if(US2>100 && US1<40) { etat=2; stop(); @@ -50,23 +57,24 @@ stop(); } break; - case 2 : + case 3 : if((AN2>10 && AN2<20) || US3<13) { etat=1; stop(); } - if(t2.read()>2.5) { + if(US1>100 && US2<10) { etat=0; + stop(); } break; - case 3 : + case 4 : if(bearing>bearing_set) { etat=4; t3.reset(); stop(); } break; - case 4 : + case 5 : if((AN2>10 && AN2<22) || US3<22) { etat=1; stop(); @@ -75,19 +83,22 @@ } switch(etat) { - case 0 : - suivi_mur(); + case 0 : + stop(); break; case 1 : - rotation_horaire(); + suivi_mur(); break; case 2 : - contournement(); + rotation_horaire(); break; case 3 : + contournement(); + break; + case 4 : rotation_horaire(); break; - case 4 : + case 5 : en_avant(); break; }