Diff: main.cpp

Revision:

0:dcb865a03d57

Child:

1:714fd6b732be

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun May 28 14:44:24 2017 +0000
@@ -0,0 +1,144 @@
+#include "mbed.h"
+#define PERIOD 0.0001
+#define VMOY 25
+#define VMAX 60
+#define Kp_E 0.0
+#define Kp_ecart 0.8
+#define Td_ecart 0.0
+#define Ti_ecart 1000.0
+#define limitmin 3
+#define limitmax 150
+#define Te 0.001
+#define Ti 2.0
+#define a 24.0
+#define b 0.1
+
+
+BusOut leds(LED1,LED2,LED3,LED4);
+DigitalOut trigger1(p14);
+DigitalOut trigger2(p16);
+DigitalOut trigger3(p18);
+InterruptIn echo(p11);
+AnalogIn AnaG(p17);
+AnalogIn AnaAV(p15);
+PwmOut MG(p21); //vitesse moteur gauche
+PwmOut MD(p24); //vitesse moteur droit
+DigitalOut sensMG(p23);  // sens moteur gauche
+DigitalOut sensMD(p26);  // sens moteur droit
+Timer temp,t;
+Ticker tic1,tic2;
+
+//GLOBALES
+int drap=1;
+float US1,US2,US3,AN1,AN2,US1_av=50,US2_av=50,US3_av=50,AN1_av=50,AN2_av=50;
+float E_av,E,iE=0;
+float cmdG=0,cmdD=0;
+int etat=0;
+float iecart=0,ecart_av,ecart;
+
+//PROTOTYPES
+void asservissement();
+void fcttrig();
+float vitesse(float);
+void start();
+void stop();
+float vitesse(float);
+void mesAN();
+
+int main()
+{
+
+    sensMG.write(1);
+    sensMD.write(1);
+    MG.period(PERIOD);
+    MD.period(PERIOD);
+    MG.pulsewidth(vitesse(0));
+    MD.pulsewidth(vitesse(0));
+    tic1.attach(&fcttrig,0.05);
+    tic2.attach(&mesAN,0.01);
+    echo.rise(&start);
+    echo.fall(&stop);
+    while(1) {
+        printf("US1=%.0f US2=%.0f US3=%.0f -- AN1=%.0f AN2=%.0f \n\r",US1,US2,US3,AN1,AN2);
+        wait(0.1);
+    }
+
+}
+void mesAN()
+{
+    AN1_av=AN1;
+    AN2_av=AN2;
+    AN1=a/(3.3*AnaG.read()-b);
+    AN2=a/(3.3*AnaAV.read()-b);
+  /*  if(((AN1-AN1_av)>50)||((AN1-AN1_av)<-50)) {
+        AN1=AN1_av;
+    }
+    if(((AN2-AN2_av)>50)||((AN2-AN2_av)<-50)) {
+        AN2=AN2_av;
+    }
+    */
+    if(AN1<0||AN1>150)AN1=150;
+    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 :
+            US3_av=US3;
+            US3=temp.read_us()/58.31;
+         //   if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
+         //       US3=US3_av;
+         //   }
+            break;
+        case 2 :
+            US2_av=US2;
+            US2=temp.read_us()/58.31;
+         //   if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
+         //       US2=US2_av;
+         //   }
+            break;
+        case 3 :
+            US1_av=US1;
+            US1=temp.read_us()/58.31;
+          //  if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
+          //      US1=US1_av;
+          //  }
+            break;
+    }
+}
+float vitesse(float vit)
+{
+    if(vit<0) vit=0;
+    if(vit>VMAX) vit=VMAX;
+    return ((vit/100.0)*PERIOD);
+}