Diff: Mlib.cpp

Revision:

4:8b0215ec788f

Parent:

3:c0aae5aa230b

Child:

5:7714a216c91d

diff -r c0aae5aa230b -r 8b0215ec788f Mlib.cpp
--- 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()