![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test de la boussole
Diff: main.cpp
- Revision:
- 2:fd17a1d6c0a3
- Parent:
- 1:714fd6b732be
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