Diff: fct.cpp

Revision:

7:2f4660e9cf92

Parent:

5:3746060957fb

--- a/fct.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/fct.cpp	Fri Jun 23 11:19:58 2017 +0000
@@ -1,68 +1,33 @@
 #include "mbed.h"
 #include "fct.h"
-void contournement()
-{
-    sensMG.write(0);
-    sensMD.write(0);
-    cmdD=35;
-    cmdG=15;
-    MD.pulsewidth(vitesse(cmdD));
-    MG.pulsewidth(vitesse(cmdG));
 
-}
 void stopMotor()
 {
-    sensMG.write(0);
-    sensMD.write(0);
-    MG.pulsewidth(vitesse(0));
-    MD.pulsewidth(vitesse(0));
+    vitesse(0,0);
 }
-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);
-    } else {
-        cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
-        cmdG=VMOY;
-    }
 
-    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()
 {
-    sensMG.write(0);
-    sensMD.write(0);
+    capb1.mode(PullUp);
+    capb2.mode(PullUp);
+    servo_stop();
+    jack.mode(PullUp);
     MG.period(PERIOD);
     MD.period(PERIOD);
-    MG.pulsewidth(vitesse(0));
-    MD.pulsewidth(vitesse(0));
+    vitesse(0,0);
     tic1.attach(&fcttrig,0.035);
     tic2.attach(&mesAN,0.01);
     echo.rise(&start);
     echo.fall(&stop);
+
 }
 void mesAN()
 {
     if(flag4==0) {
         AN1_av=AN1;
     }
-    AN1=0.82*a/(3.3*AnaG.read()-b);
-    if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
+    AN1=a/(3.3*AnaG.read()-b);
+    if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
         float temp=AN1;
         AN1=AN1_av;
         AN1_av=temp;
@@ -70,15 +35,16 @@
     } else {
         flag4=0;
     }
-    if(AN1<0||AN1>150)AN1=150;
+    if((AN1<0)||(AN1>120))AN1=120;
+
 
 
     if(flag5==0) {
         AN2_av=AN2;
     }
-    AN2=0.82*a/(3.3*AnaAV.read()-b);
+    AN2=a/(3.3*AnaAV.read()-b);
 
-    if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
+    if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
         float temp=AN2;
         AN2=AN2_av;
         AN2_av=temp;
@@ -86,9 +52,12 @@
     } else {
         flag5=0;
     }
-    if(AN2<0||AN2>150)AN2=150;
+    if((AN2<0)||(AN2>120))AN2=120;
+    capt_d=AN1;
+    capt_g=AN2;
 }
 
+
 void fcttrig()
 {
     switch(drap) {
@@ -135,7 +104,8 @@
             } else {
                 flag3=0;
             }
-            if(US3<0||US3>150)US3=150;
+            if(US3>capt_max)US3=capt_max;
+            if(US3<capt_min)US3=capt_min;
             break;
         case 2 :
             if(flag2==0) {
@@ -150,7 +120,8 @@
             } else {
                 flag2=0;
             }
-            if(US2<0||US2>150)US2=150;
+            if(US2>capt_max)US2=capt_max;
+            if(US2<capt_min)US2=capt_min;
             break;
         case 3 :
             if(flag1==0) {
@@ -165,13 +136,42 @@
             } else {
                 flag1=0;
             }
-            if(US1<0||US1>150)US1=150;
+            if(US1>capt_max)US1=capt_max;
+            if(US1<capt_min)US1=capt_min;
             break;
     }
+    capt_ed=US1;
+    capt_eg=US3;
+    capt_m=US2;
 }
-float vitesse(float vit)
+void vitesse(float vitG, float vitD)
 {
-    if(vit<0) vit=0;
-    if(vit>VMAX) vit=VMAX;
-    return ((vit/100.0)*PERIOD);
+    if(vitG<0) {
+        vitG=-1*vitG;
+        sensMG.write(1);
+    } else sensMG.write(0);
+    MG.pulsewidth(((vitG)/100.0)*PERIOD);
+    if(vitD<0) {
+        vitD=-1*vitD;
+        sensMD.write(1);
+    } else sensMD.write(0);
+    MD.pulsewidth(((vitD)/100.0)*PERIOD);
 }
+
+void servo_start(void)
+{
+    servo.period(periode);
+    turn=18.8;
+    servo.pulsewidth_ms(turn);
+    wait(0.25);
+    turn=19.8;
+    servo.pulsewidth_ms(turn);
+    wait(0.25);
+}
+
+void servo_stop(void)
+{
+    servo.period(periode);
+    turn=18.8;
+    servo.pulsewidth_ms(turn);
+}