Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
main.cpp
- Committer:
- vermaelen
- Date:
- 2017-05-28
- Revision:
- 0:dcb865a03d57
- Child:
- 1:714fd6b732be
File content as of revision 0:dcb865a03d57:
#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); }