Diff: fct.cpp

Revision:

8:24a3fa0f912a

Parent:

5:3746060957fb

--- a/fct.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/fct.cpp	Wed Jun 07 16:14:27 2017 +0000
@@ -1,177 +1,2 @@
 #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));
-}
-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);
-    MG.period(PERIOD);
-    MD.period(PERIOD);
-    MG.pulsewidth(vitesse(0));
-    MD.pulsewidth(vitesse(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)) {
-        float temp=AN1;
-        AN1=AN1_av;
-        AN1_av=temp;
-        flag4=1;
-    } else {
-        flag4=0;
-    }
-    if(AN1<0||AN1>150)AN1=150;
-
-
-    if(flag5==0) {
-        AN2_av=AN2;
-    }
-    AN2=0.82*a/(3.3*AnaAV.read()-b);
-
-    if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
-        float temp=AN2;
-        AN2=AN2_av;
-        AN2_av=temp;
-        flag5=1;
-    } else {
-        flag5=0;
-    }
-    if(AN2<0||AN2>150)AN2=150;
-}
-
-void fcttrig()
-{
-    switch(drap) {
-        case 1 :
-            trigger2.write(1);
-            wait_us(10);
-            trigger2.write(0);
-            drap=2;
-            break;
-        case 2 :
-            trigger3.write(1);
-            wait_us(10);
-            trigger3.write(0);
-            drap=3;
-            break;
-        case 3 :
-            trigger1.write(1);
-            wait_us(10);
-            trigger1.write(0);
-            drap=1;
-            break;
-    }
-
-}
-void start()
-{
-    temp.reset();
-    temp.start();
-}
-void stop()
-{
-    temp.stop();
-    switch(drap) {
-        case 1 :
-            if(flag3==0) {
-                US3_av=US3;
-            }
-            US3=temp.read_us()/58.31;
-            if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
-                float temp=US3;
-                US3=US3_av;
-                US3_av=temp;
-                flag3=1;
-            } else {
-                flag3=0;
-            }
-            if(US3<0||US3>150)US3=150;
-            break;
-        case 2 :
-            if(flag2==0) {
-                US2_av=US2;
-            }
-            US2=temp.read_us()/58.31;
-            if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
-                float temp=US2;
-                US2=US2_av;
-                US2_av=temp;
-                flag2=1;
-            } else {
-                flag2=0;
-            }
-            if(US2<0||US2>150)US2=150;
-            break;
-        case 3 :
-            if(flag1==0) {
-                US1_av=US1;
-            }
-            US1=temp.read_us()/58.31;
-            if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
-                float temp=US1;
-                US1=US1_av;
-                US1_av=temp;
-                flag1=1;
-            } else {
-                flag1=0;
-            }
-            if(US1<0||US1>150)US1=150;
-            break;
-    }
-}
-float vitesse(float vit)
-{
-    if(vit<0) vit=0;
-    if(vit>VMAX) vit=VMAX;
-    return ((vit/100.0)*PERIOD);
-}