File content as of revision 5:7714a216c91d:

#include "Mlib.h"

/***********AUTRES_FCT***************/

char middle_3(char *tab)
{
    tri_a_bulle(tab, 3);
    return tab[1];
}

int middle_3U(int *tab)
{
    tri_a_bulleU(tab, 3);
    return tab[1];
}

float middle_3F(float *tab)
{
    
    return tri_a_bulleF(tab, 3);
}


void tri_a_bulle(char* t, int const size) 
{
    int en_desordre = 1;
    int i,j;

    for (i = 0; (i < size) && en_desordre; ++i)
    {
        en_desordre = 0;
        for (j = 1; j < (size - i); ++j)
        {
            if (t[j-1] > t[j])
            {
                int temp = t[j-1];
                t[j-1] = t[j];
                t[j] = temp;
                en_desordre = 1;
            }
        }
    }
}
void tri_a_bulleU(int* t, int const size) 
{
    int en_desordre = 1;
    int i,j;

    for (i = 0; (i < size) && en_desordre; ++i)
    {
        en_desordre = 0;
        for (j = 1; j < (size - i); ++j)
        {
            if (t[j-1] > t[j])
            {
                int temp = t[j-1];
                t[j-1] = t[j];
                t[j] = temp;
                en_desordre = 1;
            }
        }
    }
}
float tri_a_bulleF(float* t2, int const size) 
{
    float t[3];
    t[0] = t2[0];
    t[1] = t2[1];
    t[2] = t2[2];
    
        
    int en_desordre = 1;
    int i,j;

    for (i = 0; (i < size) && en_desordre; ++i)
    {
        en_desordre = 0;
        for (j = 1; j < (size - i); ++j)
        {
            if (t[j-1] > t[j])
            {
                float temp = t[j-1];
                t[j-1] = t[j];
                t[j] = temp;
                en_desordre = 1;
            }
        }
    }
    return t[1];
}

unsigned char Moyenne(unsigned char *tab)
{
    int i = 0;
    int somme = 0;
    for(i = 0; i < 128;i++)
    {
        somme+=tab[i];
    }
    return somme/128.0;
}
char Moyenne2(char *tab, int nb)
{
    int i = 0;
    int somme = 0;
    for(i = 0; i < nb;i++)
    {
        somme+=tab[i];
    }
    return float(somme/nb);
}

void menu()
{
    static int p = 0;
    p++;
        wait(0.1);
        if(BP2)
        {
            seuil_der++;
            if(seuil_der == 2) seuil_der = -1;
            led1 = 0;
            wait(0.5);
        }
        if(seuil_der == 0)
        {
            led1 = p%2;
        }
        else if(seuil_der == 1)
        {
            led1 = 1;
        }
        else
        {
            led1 = 0;
        }
        if(S1 == 1)
        {
            led2 = 1;
        }
        else led2 = 0;
        if(S2 == 1)
        {
            led3 = 1;
        }
        else led3= 0;
}

void fin_course()
{
    static int arr = 0;
    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);
        
        //pc.printf("%d\n",moy_inter);
        if(S1&&(arr!=0)){//delai apres l'arret
            if (t_debut.read_ms()-arr>250)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);
                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()
//{
//    unsigned char moy = 0;
//    moy = Moyenne(cam_data);
//    if(moy > LUMI_MOYENNE && tempsInte >= MIN_INTE) tempsInte-=INCREM_BALANCE;
//    else if(tempsInte <= MAX_INTE) tempsInte+=INCREM_BALANCE;
//}