Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
Diff: main.cpp
- Revision:
- 0:dcb865a03d57
- Child:
- 1:714fd6b732be
diff -r 000000000000 -r dcb865a03d57 main.cpp --- /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); +}