Freescale_Cachan
programme course avec menu amélioré
Fork of
Programme_course_vir_am_2
by 
Freescale_Cachan
Revision 4:8b0215ec788f, committed 2017-03-22
- Comitter:
- leandre
- Date:
- Wed Mar 22 14:41:20 2017 +0000
- Parent:
- 3:c0aae5aa230b
- Commit message:
- 30 tours
Changed in this revision
--- a/Mlib.cpp Sat Mar 18 13:21:28 2017 +0000
+++ b/Mlib.cpp Wed Mar 22 14:41:20 2017 +0000
@@ -149,53 +149,113 @@
void fin_course()
{
static int arr = 0;
- static char moy_inter;
-
- /*if((somme_derivee >= (Moyenne2(last_five, 5) + (SEUIL_NB_DERRIVEE+seuil_der))) || arr != 0)
- {
- if(t_debut.read_ms() > 500 && S1 == 1 && arr == 0)
- {
- arr = t_debut.read_ms();
- }
- else if(arr != 0 && t_debut.read_ms() > arr+100)
- {
- VIT = 0;
- pc.printf("arret\ndelai:%d ms\n",(t_debut.read_ms()-arr));
- }
+ static char moy_inter, moy_mem_a,moy_mem_b;
+ static char tab_mem_b[TAILLE_TAB_INT]={0,0,0};
+ static char tab_mem_a[TAILLE_TAB_INT]={0,0,0};
+
+ if (t_debut.read_ms()>2000){
+ moy_inter=Moyenne2(Interval, TAILLE_TAB_INT);
+ moy_mem_a=Moyenne2(tab_mem_a, TAILLE_TAB_INT);
+ moy_mem_b=Moyenne2(tab_mem_b, TAILLE_TAB_INT);
-
- }
-
- for(int j = 4; j > 0; j--)
- {
- last_five[j] = last_five[j-1];
- }*/
-
- if (t_debut.read_ms()>500){
- moy_inter=Moyenne2(Interval, TAILLE_TAB_INT);
//pc.printf("%d\n",moy_inter);
-
- if((mem_a>10)&&(mem_b>10)){ // si la voiture n'est pas trop sur l'exterieur de la piste
- if(S1 && (moy_inter<70) && (moy_inter>40)){ //si l'espace entre les deux lignes est plus petit que la normale (arret)
+ if(S1&&(arr!=0)) {//delai apres l'arret
+ if (t_debut.read_ms()-arr>200)VIT=0;
+ }
+ else if((moy_mem_a>20)&&(moy_mem_b>20)){ // si la voiture n'est pas trop sur l'exterieur de la piste
+ if(S1 && (moy_inter<75) && (moy_inter>50)){ //si l'espace entre les deux lignes est plus petit que la normale (arret)
//for(int i=0;i<5;i++) pc.printf("%d ",Interval[i]);
//pc.printf("\n");
- VIT=0;
- pc.printf("\narret ");
- pc.printf("\n%d ",moy_inter);
+ //VIT=0;
+ //pc.printf("\narret ");
+ //pc.printf("\n%d ",moy_inter);
+ led1=1;
+ led2=1;
+ arr=t_debut.read_ms();
}
- }
+ }
+ else {
+ led1=0;
+ led2=0;
+
+ }
+
}
for(int j = TAILLE_TAB_INT-1; j > 0; j--)
{
Interval[j] = Interval[j-1];
- }
+ tab_mem_b[j]=tab_mem_b[j-1];
+ tab_mem_a[j]=tab_mem_a[j-1];
+ }
+ tab_mem_b[0]=mem_b;
+ tab_mem_a[0]=mem_a;
}
-
+void Gestion_bosse()
+{
+ if(BOSSE_ACTIVE)//Si on active la détection de la bosse
+ {
+
+ //pc.printf("bosse: %d , X: %1.2f , ajustement : %hi\n ",bosse,middle_3F(last_three),ajust_vit_bosse);
+
+
+ last_three[2] = last_three[1]; // les 3 dernieres valeurs de X sont stockées dans un tableau
+ last_three[1] = last_three[0];
+ last_three[0] = acc.getAccX();
+
+ if(bosse == 1)
+ {
+ ajust_vit_bosse = 60;
+ led3=1;
+ led4=0;
+
+ }
+ else if(bosse == 3){
+ ajust_vit_bosse = -80;
+ led3=0;
+ led4=1;
+
+ }
+ else
+ {
+ ajust_vit_bosse = 0;
+ led3=0;
+ led4=0;
+
+ }
+
+ if((abs(middle_3F(last_three)) > 0.13 ) && (bosse == 0)) // si montée détectée
+ {
+ bosse = 1;
+ //tep = (int)t_debut.read_ms();
+ }
+ else if(bosse == 1 && (middle_3F(last_three) > -0.1))
+ {
+ bosse = 2;
+ }
+ else if(bosse == 2 && (middle_3F(last_three) > 0.19))
+ {
+ bosse = 3;
+
+ }
+ else if(bosse==3 && (middle_3F(last_three) > 0.19))
+ {
+ ajust_vit_bosse = -50;
+
+ pc.printf("bosse: %d , X: %1.2f , ajustement : %hi\n",bosse,middle_3F(last_three),ajust_vit_bosse);
+ }
+
+ else if (bosse==3 && (middle_3F(last_three) < 0.19))
+ {
+ bosse=0;
+ ajust_vit_bosse=0;
+ }
+ }
+}
//void F_BalanceBlancs()
--- a/Mlib.h Sat Mar 18 13:21:28 2017 +0000 +++ b/Mlib.h Wed Mar 22 14:41:20 2017 +0000 @@ -15,7 +15,7 @@ #define BMIDTAB 63 #define BANDSIZE 4 #define MIN_INTE 0 -#define MAX_INTE FRQ_CYC-500 +#define MAX_INTE FRQ_CYC-400//500 #define INCREM_BALANCE 300 #define MARGE_LIGNE 5 #define MARGE_DISTANCE_LIGNE 10 @@ -31,7 +31,7 @@ #define VITESSE 40 -#define TAILLE_TAB_INT 6 +#define TAILLE_TAB_INT 5 /***********************FONCTIONS**********************/ /**/ /****************CAMERA****************/ /**/ /**/ void F_GetData(); /**/ @@ -89,6 +89,8 @@ extern DigitalIn S4; extern AnalogIn pot1; extern AnalogIn pot2; +extern int ajust_vit_bosse; +extern int ajust_vit_vir; //extern DigitalOut RGB_R(LED_RED); @@ -133,4 +135,6 @@ + + #endif /* !MLIB_H*/ \ No newline at end of file
--- a/main.cpp Sat Mar 18 13:21:28 2017 +0000
+++ b/main.cpp Wed Mar 22 14:41:20 2017 +0000
@@ -54,6 +54,8 @@
char Interval[TAILLE_TAB_INT];
char mem_a;
char mem_b;
+int ajust_vit_bosse;
+int ajust_vit_vir;
DigitalOut BP1(PTC13);//boutton A
@@ -102,6 +104,10 @@
void arret()
{
+ led1=0;
+ led2=0;
+ led3=0;
+ led4=0;
PWM_motor(MOTEUR_A, 0);//arret des moteurs
PWM_motor(MOTEUR_B, 0);
angle_servo_moteur(0);//roues droites //ici
@@ -111,6 +117,10 @@
}
while(BP1);
Init_car(); //permet de changer la vitesse avec le pot //ici
+ led1=0;
+ led2=0;
+ led3=0;
+ led4=0;
t_debut.reset();
}
@@ -123,7 +133,10 @@
menu();
}
while(BP1);//on attends le relachement de BP1
-
+ led1=0;
+ led2=0;
+ led3=0;
+ led4=0;
/***************Début de la course***************/
--- a/shield.cpp Sat Mar 18 13:21:28 2017 +0000
+++ b/shield.cpp Wed Mar 22 14:41:20 2017 +0000
@@ -7,8 +7,13 @@
{
vitesse_virage = new_bari/2;
- ajust_vitesse = (ligne_droite*5)/2;
- if(VIT == 0)
+ ajust_vitesse = ajust_vit_bosse +((ligne_droite*5)/2);-ajust_vit_vir;
+
+ pc.printf("%hi\n", ajust_vit_bosse);
+
+ if(bosse==1) {PWM_motor(90,90);} // acceleration à diminuer pour la competition
+ else if(bosse==3) {PWM_motor(30,30);}
+ else if(VIT == 0)
{
PWM_motor(0,0);
}
--- a/traitement_image.cpp Sat Mar 18 13:21:28 2017 +0000
+++ b/traitement_image.cpp Wed Mar 22 14:41:20 2017 +0000
@@ -54,25 +54,31 @@
Interval[0] = mem_a+mem_b;
if (mem_b == 100 && mem_a == 100) // pas de ligne
{
+ ajust_vit_vir=0;
baricentre = 0;
}
else if (mem_b == 100) //pas de ligne à gauche
- {
+ { //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
//baricentre = (mem_a-MARGE_DISTANCE_LIGNE);
- if(mem_a>25){
+ ajust_vit_vir=20;
+ if(mem_a>45){
baricentre = (64-mem_a);
+ ajust_vit_vir=0;
}
}
else if (mem_a == 100) // pas de ligne à droite
- {
+ { //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
//baricentre = (MARGE_DISTANCE_LIGNE-mem_b);
- if(mem_b>25){
+ ajust_vit_vir=20;
+ if(mem_b>45){
baricentre = (64-mem_b);
+ ajust_vit_vir=0;
}
}
else
{
+ ajust_vit_vir=0;
baricentre = (mem_b - mem_a);
//baricentre = -baricentre;
}
@@ -89,7 +95,7 @@
}
-void Gestion_bosse()
+/*void Gestion_bosse()
{
if(BOSSE_ACTIVE)//Si on active la détection de la bosse
{
@@ -135,7 +141,7 @@
bosse = 3;
}
}
-}
+}*/
void T_image()
{
@@ -192,6 +198,7 @@
void init_bary()
{
+ ajust_vit_vir=0;
for(int i = 0; i < 3; i++)
{
bary_tab[i] = 0;