Mlib.cpp

00001 #include "Mlib.h"
00002 
00003 void Gestion_bosse()
00004 {
00005     if(BOSSE_ACTIVE)//Si on active la détection de la bosse
00006     {
00007         led5=0;
00008         led6=0;
00009         led7=0;
00010     
00011         //pc.printf("bosse: %d , X: %1.2f , ajustement : %hi\n ",bosse,middle_3F(last_three),ajust_vit_bosse);
00012         
00013         
00014         last_three[2] = last_three[1]; // les 3 dernieres valeurs de X sont stockées dans un tableau
00015         last_three[1] = last_three[0];
00016         last_three[0] = acc.getAccX();
00017         
00018         if(bosse == 1)
00019         {
00020             ajust_vit_bosse = 60; 
00021             led6=1;
00022             led4=1;
00023         }
00024         else if(bosse == 3)
00025         ajust_vit_bosse = -80;
00026         
00027         else 
00028         {
00029             ajust_vit_bosse = 0;
00030             led7=1;
00031             led1=1;
00032         }
00033         
00034         if((abs(middle_3F(last_three)) > 0.13 ) && (bosse == 0)) // si montée détectée
00035         {
00036             bosse = 1;
00037             //tep = (int)t_debut.read_ms();
00038         }
00039         else if(bosse == 1 && (middle_3F(last_three) > -0.1)) 
00040         {
00041             bosse = 2;
00042         }
00043         else if(bosse == 2 && (middle_3F(last_three) > 0.19)) 
00044         {
00045             bosse = 3;
00046             
00047         }
00048         else if(bosse==3 && (middle_3F(last_three) > 0.19))
00049         {
00050                  ajust_vit_bosse = -50;
00051                  led5=1;
00052                  led2=1;
00053                  pc.printf("bosse: %d , X: %1.2f , ajustement : %hi\n",bosse,middle_3F(last_three),ajust_vit_bosse);
00054          }
00055                  
00056         else if (bosse==3 && (middle_3F(last_three) < 0.19)) 
00057         {
00058             bosse=0;
00059             ajust_vit_bosse=0;
00060         }
00061     }
00062 }
00063 
00064 /***********AUTRES_FCT***************/
00065 
00066 char middle_3(char *tab) // prend la valeur du milieu quand les valeurs sont rangées
00067 {
00068     tri_a_bulle(tab, 3);
00069     return tab[1];
00070 }
00071 
00072 int middle_3U(int *tab)
00073 {
00074     tri_a_bulleU(tab, 3);
00075     return tab[1];
00076 }
00077 
00078 float middle_3F(float *tab)
00079 {
00080     
00081     return tri_a_bulleF(tab, 3);
00082 }
00083 
00084 
00085 void tri_a_bulle(char* t, int const size) // permet de trier les valeurs du tableau dans l'ordre croissant
00086 {
00087     int en_desordre = 1;
00088     int i,j;
00089 
00090     for (i = 0; (i < size) && en_desordre; ++i)
00091     {
00092         en_desordre = 0;
00093         for (j = 1; j < (size - i); ++j)
00094         {
00095             if (t[j-1] > t[j])
00096             {
00097                 int temp = t[j-1];
00098                 t[j-1] = t[j];
00099                 t[j] = temp;
00100                 en_desordre = 1;
00101             }
00102         }
00103     }
00104 }
00105 void tri_a_bulleU(int* t, int const size) 
00106 {
00107     int en_desordre = 1;
00108     int i,j;
00109 
00110     for (i = 0; (i < size) && en_desordre; ++i)
00111     {
00112         en_desordre = 0;
00113         for (j = 1; j < (size - i); ++j)
00114         {
00115             if (t[j-1] > t[j])
00116             {
00117                 int temp = t[j-1];
00118                 t[j-1] = t[j];
00119                 t[j] = temp;
00120                 en_desordre = 1;
00121             }
00122         }
00123     }
00124 }
00125 float tri_a_bulleF(float* t2, int const size) 
00126 {
00127     float t[3];
00128     t[0] = t2[0];
00129     t[1] = t2[1];
00130     t[2] = t2[2];
00131     
00132         
00133     int en_desordre = 1;
00134     int i,j;
00135 
00136     for (i = 0; (i < size) && en_desordre; ++i)
00137     {
00138         en_desordre = 0;
00139         for (j = 1; j < (size - i); ++j)
00140         {
00141             if (t[j-1] > t[j])
00142             {
00143                 float temp = t[j-1];
00144                 t[j-1] = t[j];
00145                 t[j] = temp;
00146                 en_desordre = 1;
00147             }
00148         }
00149     }
00150     return t[1];
00151 }
00152 
00153 unsigned char Moyenne(unsigned char *tab)
00154 {
00155     int i = 0;
00156     int somme = 0;
00157     for(i = 0; i < 128;i++)
00158     {
00159         somme+=tab[i];
00160     }
00161     return somme/128.0;
00162 }
00163 char Moyenne2(char *tab, int nb)
00164 {
00165     int i = 0;
00166     int somme = 0;
00167     for(i = 0; i < nb;i++)
00168     {
00169         somme+=tab[i];
00170     }
00171     return float(somme/nb);
00172 }
00173 
00174 void menu()
00175 {
00176     static int p = 0;
00177     p++;
00178         wait(0.1);
00179         if(BP2) // bouton B
00180         {
00181             seuil_der++;
00182             if(seuil_der == 2) seuil_der = -1;
00183             led1 = 0;
00184             wait(0.5);
00185         }
00186         if(seuil_der == 0)
00187         {
00188             led1 = p%2;
00189         }
00190         else if(seuil_der == 1)
00191         {
00192             led1 = 1;
00193         }
00194         else
00195         {
00196             led1 = 0;
00197         }
00198         if(S1 == 1)
00199         {
00200             led2 = 1;
00201         }
00202         else led2 = 0;
00203         if(S2 == 1)
00204         {
00205             led3 = 1;
00206         }
00207         else led3= 0;
00208 }
00209 
00210 void fin_course()
00211 {
00212     static int arr = 0;
00213     if(somme_derivee >= (Moyenne2(last_five, 5) + (SEUIL_NB_DERRIVEE+5*seuil_der)) || arr != 0)
00214     {
00215         if(t_debut.read() > 3 && S1 == 1 && arr == 0)
00216         {
00217            arr = t_debut.read_ms();
00218         }
00219         else if(arr != 0 && t_debut.read_ms() > arr+100)
00220         {
00221             VIT = 0;
00222         }
00223     }
00224     for(int j = 4; j > 0; j--)
00225     {
00226         last_five[j] = last_five[j-1];
00227 //                pc.printf("%d\n", last_five[j]);
00228     }
00229 //            pc.printf("%d     |     %d\n", (somme_derivee), (Moyenne2(last_five, 5)));
00230     last_five[0] = somme_derivee;
00231 }
00232 
00233 
00234 
00235 
00236 
00237 //void F_BalanceBlancs()
00238 //{
00239 //    unsigned char moy = 0;
00240 //    moy = Moyenne(cam_data);
00241 //    if(moy > LUMI_MOYENNE && tempsInte >= MIN_INTE) tempsInte-=INCREM_BALANCE;
00242 //    else if(tempsInte <= MAX_INTE) tempsInte+=INCREM_BALANCE;
00243 //}
00244 
00245 
00246