Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
Diff: fct.cpp
- Revision:
- 3:b91371837109
- Parent:
- 2:82b72fa8dbcd
- Child:
- 4:78a9354fcee8
--- a/fct.cpp Sun May 28 17:36:57 2017 +0000 +++ b/fct.cpp Mon May 29 12:01:43 2017 +0000 @@ -1,5 +1,15 @@ #include "mbed.h" #include "fct.h" +void contournement() +{ + sensMG.write(0); + sensMD.write(0); + cmdD=30; + cmdG=10; + MD.pulsewidth(vitesse(cmdD)); + MG.pulsewidth(vitesse(cmdG)); + +} void stopMotor() { sensMG.write(0); @@ -9,11 +19,17 @@ } void suivi_mur() { - cmdD=1.05*VMOY+Kp_E*(US2-US1); - cmdG=VMOY-Kp_E*(US2-US1); - + E3=E2; + E2=E1; + E1=E0; + E0=US2-US1; + cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-20); + cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-20); + MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); + + wait(0.001); } void rotation_horaire() { @@ -30,7 +46,7 @@ MD.period(PERIOD); MG.pulsewidth(vitesse(0)); MD.pulsewidth(vitesse(0)); - tic1.attach(&fcttrig,0.033); + tic1.attach(&fcttrig,0.035); tic2.attach(&mesAN,0.01); echo.rise(&start); echo.fall(&stop); @@ -41,7 +57,7 @@ AN1_av=AN1; } AN1=0.82*a/(3.3*AnaG.read()-b); - if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) { + if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) { float temp=AN1; AN1=AN1_av; AN1_av=temp; @@ -57,7 +73,7 @@ } AN2=0.82*a/(3.3*AnaAV.read()-b); - if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) { + if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) { float temp=AN2; AN2=AN2_av; AN2_av=temp;