Diff: fct.cpp

Revision:

7:64bfecbb3000

Parent:

5:3746060957fb

Child:

9:7f62c0329849

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