christophe vermaelen
/
_test_suivi_mur
...
Diff: main.cpp
- Revision:
- 2:82b72fa8dbcd
- Parent:
- 1:714fd6b732be
- Child:
- 3:b91371837109
diff -r 714fd6b732be -r 82b72fa8dbcd main.cpp --- a/main.cpp Sun May 28 14:44:51 2017 +0000 +++ b/main.cpp Sun May 28 17:36:57 2017 +0000 @@ -1,145 +1,35 @@ #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(); +#include "fct.h" 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); + int etat=0; + init(); 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; -} + //printf("etat=%d US1=%.0f US2=%.0f erreur=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,(US2-US1),cmdD,cmdG); + //wait(0.05); + switch(etat) { + case 0 : + if(AN2<20 || US3<13) { + etat=1; + stopMotor(); + } + break; + case 1 : + if(AN2>30) { + etat=0; + stopMotor(); + } + break; + } + switch(etat) { + case 0 : + suivi_mur(); + break; + case 1 : + rotation_horaire(); + break; + } -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); -}