Diff: main.cpp

Revision:

2:fd17a1d6c0a3

Parent:

1:714fd6b732be

--- a/main.cpp	Sun May 28 14:44:51 2017 +0000
+++ b/main.cpp	Wed May 31 08:49:23 2017 +0000
@@ -1,49 +1,25 @@
 #include "mbed.h"
+#include "CMPS03.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
 
+CMPS03 boussole(p9,p10,CMPS03_DEFAULT_I2C_ADDRESS);
 
 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;
+float vitesse(float vit)
+{
+    if(vit<0) vit=0;
+    if(vit>60) vit=60;
+    return ((vit/100.0)*PERIOD);
+}
 
-//PROTOTYPES
-void asservissement();
-void fcttrig();
-float vitesse(float);
-void start();
-void stop();
-float vitesse(float);
-void mesAN();
+
 
 int main()
 {
@@ -54,92 +30,11 @@
     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);
+        
+        printf("CAP=%.1f \n\r",boussole.readBearing()/10.0);
         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);
-}
+}
\ No newline at end of file