Alexandre Pirotte / Mbed 2 deprecated 0concours_cachan_programme_ok

ok

Dependencies:   mbed

Fork of _test_suivi_mur by christophe vermaelen

Diff: fct.cpp

Revision:
5:3746060957fb
Parent:
4:78a9354fcee8
Child:
7:2f4660e9cf92
Child:
8:24a3fa0f912a
--- a/fct.cpp	Mon May 29 16:50:40 2017 +0000
+++ b/fct.cpp	Mon May 29 16:52:08 2017 +0000
@@ -23,8 +23,13 @@
     E2=E1;
     E1=E0;
     E0=US2-US1;
-    cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
-    cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-45);
+    if((E0+E1)>0) {
+        cmdD=VMOY;
+        cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-45);
+    } else {
+        cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
+        cmdG=VMOY;
+    }
 
     MD.pulsewidth(vitesse(cmdD));
     MG.pulsewidth(vitesse(cmdG));