tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
Diff: main.cpp
- Revision:
- 7:64bfecbb3000
- Parent:
- 6:5f7df5c74a77
- Child:
- 8:72d684a69957
- Child:
- 9:7f62c0329849
--- a/main.cpp Wed May 31 08:48:23 2017 +0000 +++ b/main.cpp Wed May 31 12:17:59 2017 +0000 @@ -1,5 +1,7 @@ -#include "mbed.h" + #include "fct.h" + + BusOut ledsetat(p12,p13); int main() @@ -7,42 +9,57 @@ 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); + // 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); + //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; - stopMotor(); + stop(); } if(US2>100 && US1<40) { etat=2; - stopMotor(); + stop(); } break; case 1 : if(US2>100 && US1<40) { etat=2; - stopMotor(); + stop(); } if(AN2>30) { etat=0; t2.start(); t2.reset(); - stopMotor(); + stop(); } - + if(bearing>(bearing-90.0)) { + etat=0; + stop(); + } break; case 2 : if((AN2>10 && AN2<20) || US3<13) { etat=1; - stopMotor(); + stop(); } if(t2.read()>2.5) { etat=0; } break; + } switch(etat) { case 0 :