christophe vermaelen
/
_test_suivi_mur
...
Diff: main.cpp
- Revision:
- 3:b91371837109
- Parent:
- 2:82b72fa8dbcd
- Child:
- 4:78a9354fcee8
--- a/main.cpp Sun May 28 17:36:57 2017 +0000 +++ b/main.cpp Mon May 29 12:01:43 2017 +0000 @@ -1,26 +1,43 @@ #include "mbed.h" #include "fct.h" +BusOut ledsetat(p12,p13); int main() { int etat=0; init(); while(1) { - //printf("etat=%d US1=%.0f US2=%.0f erreur=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,(US2-US1),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); + ledsetat.write(etat); switch(etat) { case 0 : - if(AN2<20 || US3<13) { + if((AN2>10 && AN2<20) || US3<13) { etat=1; stopMotor(); } + if(US2>100 && US1<40) { + etat=2; + stopMotor(); + } break; case 1 : if(AN2>30) { etat=0; + t2.start(); + t2.reset(); stopMotor(); } break; + case 2 : + if((AN2>10 && AN2<20) || US3<13) { + etat=1; + stopMotor(); + } + if(t2.read()>2) { + etat=0; + } + break; } switch(etat) { case 0 : @@ -29,6 +46,9 @@ case 1 : rotation_horaire(); break; + case 2 : + contournement(); + break; } }