Diff: fct.cpp

Revision:

3:b91371837109

Parent:

2:82b72fa8dbcd

Child:

4:78a9354fcee8

--- a/fct.cpp	Sun May 28 17:36:57 2017 +0000
+++ b/fct.cpp	Mon May 29 12:01:43 2017 +0000
@@ -1,5 +1,15 @@
 #include "mbed.h"
 #include "fct.h"
+void contournement()
+{
+    sensMG.write(0);
+    sensMD.write(0);
+    cmdD=30;
+    cmdG=10;
+    MD.pulsewidth(vitesse(cmdD));
+    MG.pulsewidth(vitesse(cmdG));
+
+}
 void stopMotor()
 {
     sensMG.write(0);
@@ -9,11 +19,17 @@
 }
 void suivi_mur()
 {
-    cmdD=1.05*VMOY+Kp_E*(US2-US1);
-    cmdG=VMOY-Kp_E*(US2-US1);
-    
+    E3=E2;
+    E2=E1;
+    E1=E0;
+    E0=US2-US1;
+    cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-20);
+    cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-20);
+
     MD.pulsewidth(vitesse(cmdD));
     MG.pulsewidth(vitesse(cmdG));
+
+    wait(0.001);
 }
 void rotation_horaire()
 {
@@ -30,7 +46,7 @@
     MD.period(PERIOD);
     MG.pulsewidth(vitesse(0));
     MD.pulsewidth(vitesse(0));
-    tic1.attach(&fcttrig,0.033);
+    tic1.attach(&fcttrig,0.035);
     tic2.attach(&mesAN,0.01);
     echo.rise(&start);
     echo.fall(&stop);
@@ -41,7 +57,7 @@
         AN1_av=AN1;
     }
     AN1=0.82*a/(3.3*AnaG.read()-b);
-    if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
+    if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
         float temp=AN1;
         AN1=AN1_av;
         AN1_av=temp;
@@ -57,7 +73,7 @@
     }
     AN2=0.82*a/(3.3*AnaAV.read()-b);
 
-    if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
+    if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
         float temp=AN2;
         AN2=AN2_av;
         AN2_av=temp;