tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
Diff: fct.cpp
- Revision:
- 7:64bfecbb3000
- Parent:
- 5:3746060957fb
- Child:
- 9:7f62c0329849
diff -r 5f7df5c74a77 -r 64bfecbb3000 fct.cpp --- a/fct.cpp Wed May 31 08:48:23 2017 +0000 +++ b/fct.cpp Wed May 31 12:17:59 2017 +0000 @@ -1,5 +1,6 @@ #include "mbed.h" #include "fct.h" +#include "CMPS03.h" void contournement() { sensMG.write(0); @@ -10,38 +11,41 @@ MG.pulsewidth(vitesse(cmdG)); } -void stopMotor() +void rotation_horaire() +{ + sensMG.write(0); + sensMD.write(1); + cmdD=30; + cmdG=30; + MD.pulsewidth(vitesse(cmdD)); + MG.pulsewidth(vitesse(cmdG)); +} +void stop() { sensMG.write(0); sensMD.write(0); - MG.pulsewidth(vitesse(0)); - MD.pulsewidth(vitesse(0)); + cmdD=0; + cmdG=0; + MD.pulsewidth(vitesse(cmdD)); + MG.pulsewidth(vitesse(cmdG)); } void suivi_mur() { - E3=E2; - E2=E1; E1=E0; E0=US2-US1; if((E0+E1)>0) { - cmdD=VMOY; - cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-45); + cmdD=VMAX; + cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-45); } else { - cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-45); - cmdG=VMOY; + cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-45); + cmdG=VMAX; } MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); wait(0.001); -} -void rotation_horaire() -{ - sensMG.write(0); - sensMD.write(1); - MG.pulsewidth(vitesse(30)); - MD.pulsewidth(vitesse(30)); + } void init() { @@ -53,8 +57,9 @@ MD.pulsewidth(vitesse(0)); tic1.attach(&fcttrig,0.035); tic2.attach(&mesAN,0.01); - echo.rise(&start); - echo.fall(&stop); + echo.rise(&start_trig); + echo.fall(&stop_trig); + bearing_set=boussole.readBearing()/10.0; } void mesAN() { @@ -113,12 +118,12 @@ } } -void start() +void start_trig() { temp.reset(); temp.start(); } -void stop() +void stop_trig() { temp.stop(); switch(drap) { @@ -172,6 +177,6 @@ float vitesse(float vit) { if(vit<0) vit=0; - if(vit>VMAX) vit=VMAX; + if(vit>VLIMIT) vit=VLIMIT; return ((vit/100.0)*PERIOD); }