test de la boussole

Dependencies:   CMPS03 mbed

Files at this revision

API Documentation at this revision

Comitter:
vermaelen
Date:
Wed May 31 08:49:23 2017 +0000
Parent:
1:714fd6b732be
Commit message:
test boussole OK

Changed in this revision

CMPS03.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 714fd6b732be -r fd17a1d6c0a3 CMPS03.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CMPS03.lib	Wed May 31 08:49:23 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/CMPS03/#c6bcc390612a
diff -r 714fd6b732be -r fd17a1d6c0a3 main.cpp
--- 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