tuan din engku syasya syazwani
ROBOTIC COMPETITION PARIS 2017
Fork of
_test_suivi_mur
by 
christophe vermaelen
Revision 13:4fdd8ad91c69, committed 2018-01-22
- Comitter:
- syasya
- Date:
- Mon Jan 22 19:32:00 2018 +0000
- Parent:
- 12:6151eb503170
- Commit message:
- ROBOTIQUE COMPETITION PARIS
Changed in this revision
diff -r 6151eb503170 -r 4fdd8ad91c69 fct.cpp
--- a/fct.cpp Thu Jun 01 09:30:37 2017 +0000
+++ b/fct.cpp Mon Jan 22 19:32:00 2018 +0000
@@ -1,6 +1,34 @@
#include "mbed.h"
#include "fct.h"
#include "CMPS03.h"
+
+void ballon()
+{
+ stop();
+ servo.period(0.02);
+ servo.pulsewidth_ms(17);
+ wait(0.25);
+ servo.pulsewidth_ms(19.5);
+ wait(0.25);
+}
+void suivi_mur_dist()
+{
+ E1=E0;
+ E0=US2-US1;
+ if((E0+E1)>0) {
+ cmdD=VMAX;
+ cmdG=VMAX-Kp_ecart*(E0+E1)-Kp_dist*(US2-30);
+ } else {
+ cmdD=VMAX+Kp_ecart*(E0+E1)+Kp_dist*(US2-30);
+ cmdG=VMAX;
+ }
+
+ MD.pulsewidth(vitesse(cmdD));
+ MG.pulsewidth(vitesse(cmdG));
+
+ wait(0.001);
+
+}
void rotation_sharp()
{
sensMG.write(0);
@@ -14,8 +42,8 @@
{
sensMG.write(0);
sensMD.write(0);
- cmdD=35;
- cmdG=35;
+ cmdD=60;
+ cmdG=60;
MD.pulsewidth(vitesse(cmdD));
MG.pulsewidth(vitesse(cmdG));
}
@@ -23,8 +51,8 @@
{
sensMG.write(0);
sensMD.write(0);
- cmdD=35;
- cmdG=15;
+ cmdD=40;
+ cmdG=22;
MD.pulsewidth(vitesse(cmdD));
MG.pulsewidth(vitesse(cmdG));
@@ -33,8 +61,8 @@
{
sensMG.write(0);
sensMD.write(0);
- cmdD=15;
- cmdG=35;
+ cmdD=30;
+ cmdG=40;
MD.pulsewidth(vitesse(cmdD));
MG.pulsewidth(vitesse(cmdG));
}
@@ -66,7 +94,8 @@
}
void init()
-{
+{
+ smoke.write(0);
sensMG.write(0);
sensMD.write(0);
MG.period(PERIOD);
diff -r 6151eb503170 -r 4fdd8ad91c69 fct.h --- a/fct.h Thu Jun 01 09:30:37 2017 +0000 +++ b/fct.h Mon Jan 22 19:32:00 2018 +0000 @@ -7,33 +7,38 @@ //GLOBALES extern BusOut leds; extern DigitalIn jack; +extern DigitalIn capt2; +extern DigitalIn capt1; +extern DigitalOut smoke; extern DigitalOut trigger1; extern DigitalOut trigger2; extern DigitalOut trigger3; extern InterruptIn echo; extern AnalogIn AnaG; extern AnalogIn AnaAV; +extern PwmOut servo; extern PwmOut MG; //vitesse moteur gauche extern PwmOut MD; //vitesse moteur droit extern DigitalOut sensMG; // sens moteur gauche extern DigitalOut sensMD; // sens moteur droit -extern Timer temp,t,t2,t3; -extern Ticker tic1,tic2; +extern Timer temp,t,t2,t3,t4,t5; +extern Ticker tic1,tic2,tic3; extern CMPS03 boussole; - -extern int etat,t_set; +extern int nombre; +extern int capt,capt_memo; +extern int etat,t3_set; extern int drap,flag1,flag2,flag3,flag4,flag5; -extern float US1,US2,US3,AN1,AN2,US1_av,US2_av,US3_av,AN1_av,AN2_av; +extern float US1,US2,US3,AN1,AN2,US1_av,US2_av,US3_av,AN1_av,AN2_av,AN3; extern float E_av,E,iE,E0,E1; extern float cmdG,cmdD; extern float iecart,ecart_av,ecart; -extern float bearing,bearing_set; +extern float bearing,bearing_set,bearing_turn; //CONSTANTES #define PERIOD 0.0001 -#define VMAX 30 +#define VMAX 60 #define VLIMIT 70 #define Kp_dist 0.2 #define Kp_ecart 0.22 @@ -47,13 +52,15 @@ void stop(); void init(); void suivi_mur(); +void suivi_mur_dist(); void contournement(); void rotation_horaire(); void rotation_sharp(); void fcttrig(); void mesAN(); +void ballon(); +void fume(); float vitesse(float); - #endif
diff -r 6151eb503170 -r 4fdd8ad91c69 globals.cpp --- a/globals.cpp Thu Jun 01 09:30:37 2017 +0000 +++ b/globals.cpp Mon Jan 22 19:32:00 2018 +0000 @@ -2,26 +2,32 @@ #include "CMPS03.h" BusOut leds(LED1,LED2,LED3,LED4); CMPS03 boussole(p9,p10,CMPS03_DEFAULT_I2C_ADDRESS); -DigitalIn jack(p25); +DigitalIn jack(p12); +DigitalIn capt1(p29); +DigitalIn capt2(p30); +DigitalOut smoke(p13); DigitalOut trigger1(p14); DigitalOut trigger2(p16); DigitalOut trigger3(p18); InterruptIn echo(p11); AnalogIn AnaG(p17); AnalogIn AnaAV(p15); +PwmOut servo(p22); PwmOut MD(p21); //vitesse moteur gauche PwmOut MG(p24); //vitesse moteur droit DigitalOut sensMG(p23); // sens moteur gauche DigitalOut sensMD(p26); // sens moteur droit -Timer temp,t,t2,t3; -Ticker tic1,tic2; +Timer temp,t,t2,t3,t4,t5; +Ticker tic1,tic2,tic3; -int t_set=10; +int t3_set=10; +int capt_memo=0,capt=0; int drap=1,flag1=0,flag2=0,flag3=0,flag4=0,flag5=0; -float US1,US2,US3,AN1,AN2,US1_av=50,US2_av=50,US3_av=50,AN1_av=50,AN2_av=50; +float US1,US2,US3,AN1,AN2,US1_av=50,US2_av=50,US3_av=50,AN1_av=50,AN2_av=50,AN3=0; float E_av,E,iE=0,E0=0,E1=0; float cmdG=0,cmdD=0; int etat=0; float iecart=0,ecart_av,ecart; -float bearing,bearing_set; +float bearing,bearing_set,bearing_turn; +int nombre=0; \ No newline at end of file
diff -r 6151eb503170 -r 4fdd8ad91c69 main.cpp
--- a/main.cpp Thu Jun 01 09:30:37 2017 +0000
+++ b/main.cpp Mon Jan 22 19:32:00 2018 +0000
@@ -1,9 +1,5 @@
-
#include "fct.h"
-
-BusOut ledsetat(p12,p13);
-
int main()
{
@@ -11,48 +7,48 @@
int etat=0;
init();
t3.reset();
+ t3.stop();
while(1) {
- // printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG);
- //wait(0.05);
- //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing);
+ //printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f jack=%d\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG,jack.read());
//wait(0.1);
-
- bearing=boussole.readBearing()/10.0;
- ledsetat.write(etat);
-
- if(bearing<bearing_set+10.0 && bearing>bearing_set-10.0) {
- t3.start();
- } else {
- t3.stop();
- }
+ //printf("etat=%d set=%.1f CAP=%.1f t3=%f capt1=%d capt2=%d\n\r",etat,bearing_set,bearing,t3.read(),capt1.read(),capt2.read());
+ //wait(0.1);
+ capt=capt2.read();
switch(etat) {
case 0 :
- if(jack.read()==0) {
+ if(jack.read()==1) {
etat=1;
t3.reset();
+ t3.start();
+ t5.reset();
+ t5.start();
}
break;
case 1 :
- if(jack.read()!=0) etat=0;
- if((AN2>40 && AN2<45) || US3<40) {
+
+ if(jack.read()!=1) etat=0;
+ if((AN2>10 && AN2<40) || US3<18) {
etat=2;
stop();
}
- if(US2>100 && AN1<40) {
+ if(US2>100 && US1<40) {
etat=3;
stop();
}
- if(t3.read()>8) {
+ /*if(t3.read()>7) {
etat=4;
- bearing_set=bearing_set+25.0;
- t3.reset();
- t3.stop();
+ stop();
+ }*/
+ if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+ etat=9;
+ t4.reset();
+ t4.start();
stop();
}
break;
case 2 :
- if(jack.read()!=0) etat=0;
+ if(jack.read()!=1) etat=0;
if(US2>100 && US1<40) {
etat=3;
stop();
@@ -63,33 +59,108 @@
t2.reset();
stop();
}
+ if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+ etat=9;
+ t4.reset();
+ t4.start();
+ stop();
+ }
break;
case 3 :
- if(jack.read()!=0) etat=0;
- if((AN2>40 && AN2<45) || US3<40) {
+ if(jack.read()!=1) etat=0;
+ if((AN2>10 && AN2<30) || US3<18) {
etat=2;
stop();
}
- if(t2.read()>2) {
+ if(t2.read()>5) {
etat=1;
stop();
}
+ if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+ etat=9;
+ t4.reset();
+ t4.start();
+ stop();
+ }
break;
case 4 :
- if(jack.read()!=0) etat=0;
- if(bearing>bearing_set) {
- etat=5;
- t3.reset();
+ etat=5;
+ t3.reset();
+ stop();
+
+ break;
+ case 5 :
+ if(US3<40 || US2<50) {
+ etat=6;
stop();
}
break;
- case 5 :
- if(jack.read()!=0) etat=0;
- if((AN2>40 && AN2<42) || US3<40) {
- etat=2;
+ case 6 :
+ t3.start();
+ if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+ etat=9;
+ t4.reset();
+ t4.start();
+ stop();
+ }
+ if(jack.read()!=1) etat=0;
+ if((AN2>10 && AN2<30) || US3<18) {
+ etat=7;
+ stop();
+ }
+ if(US2>100 && US1<40) {
+ etat=8;
+ stop();
+ }
+ if(t3.read()>58) {
+ etat=9;
+ }
+ break;
+ case 7 :
+ if(jack.read()!=1) etat=0;
+ if(US2>100 && US1<40) {
+ etat=8;
+ stop();
+ }
+ if(AN2>30) {
+ etat=6;
+ t2.start();
+ t2.reset();
+ stop();
+ }
+ if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+ etat=9;
+ t4.reset();
+ t4.start();
stop();
}
break;
+ case 8 :
+ if(jack.read()!=1) etat=0;
+ if((AN2>10 && AN2<30) || US3<18) {
+ etat=7;
+ stop();
+ }
+ if(t2.read()>4) {
+ etat=6;
+ stop();
+ }
+ if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+ etat=9;
+ t4.reset();
+ t4.start();
+ stop();
+ }
+ break;
+ case 9 :
+ if(t4.read()>2) etat=10;
+ stop();
+ break;
+ case 10 :
+ if(jack.read()!=1) etat=0;
+ stop();
+ break;
+
}
switch(etat) {
@@ -106,11 +177,31 @@
contournement();
break;
case 4 :
- rotation_sharp();
+ rotation_horaire();
+ wait_ms(700);
break;
case 5 :
en_avant();
break;
+ case 6 :
+ suivi_mur_dist();
+ break;
+ case 7 :
+ rotation_sharp();
+ break;
+ case 8 :
+ contournement();
+ break;
+ case 9 :
+ ballon();
+ smoke.write(1);
+ break;
+ case 10 :
+ stop();
+ if(t4.read()>3) {
+ smoke.write(0);
+ }
+ break;
}
}