Diff: fct.cpp

Revision:

2:82b72fa8dbcd

Child:

3:b91371837109

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fct.cpp	Sun May 28 17:36:57 2017 +0000
@@ -0,0 +1,156 @@
+#include "mbed.h"
+#include "fct.h"
+void stopMotor()
+{
+    sensMG.write(0);
+    sensMD.write(0);
+    MG.pulsewidth(vitesse(0));
+    MD.pulsewidth(vitesse(0));
+}
+void suivi_mur()
+{
+    cmdD=1.05*VMOY+Kp_E*(US2-US1);
+    cmdG=VMOY-Kp_E*(US2-US1);
+    
+    MD.pulsewidth(vitesse(cmdD));
+    MG.pulsewidth(vitesse(cmdG));
+}
+void rotation_horaire()
+{
+    sensMG.write(0);
+    sensMD.write(1);
+    MG.pulsewidth(vitesse(20));
+    MD.pulsewidth(vitesse(20));
+}
+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.033);
+    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)>50)||((AN1-AN1_av)<-50)) {
+        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)>50)||((AN2-AN2_av)<-50)) {
+        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);
+}