tuan din engku syasya syazwani
/
CACHAN_D_DAY_2
ROBOTIC COMPETITION PARIS 2017
Fork of _test_suivi_mur by
Diff: fct.cpp
- Revision:
- 13:4fdd8ad91c69
- Parent:
- 12:6151eb503170
diff -r 6151eb503170 -r 4fdd8ad91c69 fct.cpp --- a/fct.cpp Thu Jun 01 09:30:37 2017 +0000 +++ b/fct.cpp Mon Jan 22 19:32:00 2018 +0000 @@ -1,6 +1,34 @@ #include "mbed.h" #include "fct.h" #include "CMPS03.h" + +void ballon() +{ + stop(); + servo.period(0.02); + servo.pulsewidth_ms(17); + wait(0.25); + servo.pulsewidth_ms(19.5); + wait(0.25); +} +void suivi_mur_dist() +{ + E1=E0; + E0=US2-US1; + if((E0+E1)>0) { + cmdD=VMAX; + cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-30); + } else { + cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-30); + cmdG=VMAX; + } + + MD.pulsewidth(vitesse(cmdD)); + MG.pulsewidth(vitesse(cmdG)); + + wait(0.001); + +} void rotation_sharp() { sensMG.write(0); @@ -14,8 +42,8 @@ { sensMG.write(0); sensMD.write(0); - cmdD=35; - cmdG=35; + cmdD=60; + cmdG=60; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } @@ -23,8 +51,8 @@ { sensMG.write(0); sensMD.write(0); - cmdD=35; - cmdG=15; + cmdD=40; + cmdG=22; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); @@ -33,8 +61,8 @@ { sensMG.write(0); sensMD.write(0); - cmdD=15; - cmdG=35; + cmdD=30; + cmdG=40; MD.pulsewidth(vitesse(cmdD)); MG.pulsewidth(vitesse(cmdG)); } @@ -66,7 +94,8 @@ } void init() -{ +{ + smoke.write(0); sensMG.write(0); sensMD.write(0); MG.period(PERIOD);