Diff: fct.cpp

Revision:

13:4fdd8ad91c69

Parent:

12:6151eb503170

--- 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);