Freescale_Cachan
Programme course fonctionnel Jour J Croisement non pris en charge (tourne en rond)
Fork of
Programme_course_30Tr
by 
Freescale_Cachan
Diff: Mlib.cpp
- Revision:
- 4:8b0215ec788f
- Parent:
- 3:c0aae5aa230b
- Child:
- 5:7714a216c91d
--- 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()