Diff: main.cpp

Revision:

2:82b72fa8dbcd

Parent:

1:714fd6b732be

Child:

3:b91371837109

--- a/main.cpp	Sun May 28 14:44:51 2017 +0000
+++ b/main.cpp	Sun May 28 17:36:57 2017 +0000
@@ -1,145 +1,35 @@
 #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();
+#include "fct.h"
 
 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);
+    int etat=0;
+    init();
     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;
-}
+        //printf("etat=%d US1=%.0f US2=%.0f erreur=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,(US2-US1),cmdD,cmdG);
+        //wait(0.05);
+        switch(etat) {
+            case 0 :
+                if(AN2<20 || US3<13) {
+                    etat=1;
+                    stopMotor();
+                }
+                break;
+            case 1 :
+                if(AN2>30) {
+                    etat=0;
+                    stopMotor();
+                }
+                break;
+        }
+        switch(etat) {
+            case 0 :
+                suivi_mur();
+                break;
+            case 1 :
+                rotation_horaire();
+                break;
+        }
 
-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);
-}