File content as of revision 4:8b0215ec788f:

#include "Mlib.h"

/**********TRAITEMENT_IMG************/

void F_TraitementImage()
{    
    unsigned char i;
    signed short save_a;
    signed short save_b;
    
    int tempo = 0;
    char tab[3];
    char tab2[3];
    mem_a = 100;
    mem_b = 100;
    save_b = MARGE_LIGNE;
    save_a = MARGE_LIGNE;
    somme_derivee = 0;
     for(i = 0; i < 62-4; i++) //analyse de la partie droite de la piste
    {
        tab[0] = cam_data[AMIDTAB+i];
        tab[1] = cam_data[AMIDTAB+i+1];
        tab[2] = cam_data[AMIDTAB+i+2];
        tab2[0] = cam_data[AMIDTAB+i+3];
        tab2[1] = cam_data[AMIDTAB+i+4];
        tab2[2] = cam_data[AMIDTAB+i+5];
        tempo = middle_3(tab)-middle_3(tab2);
        if(tempo < 0) tempo = 0;
        somme_derivee+= tempo;
        if(save_a < tempo)
        {
            save_a = tempo;
            mem_a = i;
        }
    }
    for(i = 1; i < 63-4; i++) //analyse de la partie gauche de la piste
    {
        tab[0] = cam_data[AMIDTAB-i];
        tab[1] = cam_data[AMIDTAB-i-1];
        tab[2] = cam_data[AMIDTAB-i-2];
        tab2[0] = cam_data[AMIDTAB-i-3];
        tab2[1] = cam_data[AMIDTAB-i-4];
        tab2[2] = cam_data[AMIDTAB-i-5];
        tempo = middle_3(tab)-middle_3(tab2);
        if(tempo < 0) tempo = 0;
        somme_derivee+= tempo;
        if(save_b < tempo)
        {
            save_b = tempo;
            mem_b = i;
        }
    }
    
    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);
        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);
        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;
    }
} 

void F_BalanceBlancs()
{
    static unsigned char moy = 0;
    moy = Moyenne(cam_data);
    tempsInte+=100*(LUMI_MOYENNE-moy);
    if(tempsInte>MAX_INTE) tempsInte = MAX_INTE;
    if(tempsInte<MIN_INTE) tempsInte = MIN_INTE;
    somme_derivee = somme_derivee/(moy/10.0);
}


/*void Gestion_bosse()
{
    if(BOSSE_ACTIVE)//Si on active la détection de la bosse
    {
        static int tep = 0;
        switch(bosse)
        {
            case 0:
            led1 = 1;
            led2 = 0;
            break;
            case 1:
            led2 = 1;
            led1= 0;
            break;
            case 2:
            led3 = 1;
            led2 = 0;
            break;
        }
    
        
        last_three[2] = last_three[1];
        last_three[1] = last_three[0];
        last_three[0] = acc.getAccX();
        
        if(bosse == 1)
        ajust_vitesse = 20;
        else if(bosse == 2)
        ajust_vitesse = -10;
        else ajust_vitesse = 0;
        
        if((middle_3F(last_three) > 0.2 ) && (bosse == 0 && t_debut.read() > 2))
        {
            bosse = 1;
            tep = (int)t_debut.read_ms();
        }
        else if(bosse == 1 && (int)t_debut.read_ms() > (tep+1000))
        {
            bosse = 2;
        }
        else if(bosse == 2 && middle_3F(last_three) > 0.98)
        {
            bosse = 3;
        }
    }
}*/

void T_image()
{
    static int  last_bari = 0, bari_bis = 0;
    static int retiens = 0;
    F_TraitementImage();//donne une valeur au baricentre
    
    bary_tab[2] = bary_tab[1];//décalage du tableau des 3 derniers bari
    bary_tab[1] = bary_tab[0];
    bary_tab[0] = baricentre;
    
    for(int i = 0; i < 3; i++)//on rentre le tableau dans un autre tableau
    {
        bary_tab_bis[i] = bary_tab[i];
    }
    
    bari_bis = middle_3U(bary_tab_bis);//On calcul la médianne des 3 derniers bari
    integral += bari_bis;
    new_bari = (KI*integral+KP*bari_bis+KD*(bari_bis-last_bari))/1000;//On applique un KP et un KD (proportionnel et dérivé) au bari
    //pc.printf("%d  ",new_bari);
    last_bari = bari_bis;
    
    led4 = ralentire;
    if(S3)//gestion des lignes droite
    {
        if(abs(new_bari) < 15 && ligne_droite < 10 && ralentire == 0)
        {
            if(ligne_droite == 0)
            {
                t_ligne.reset();
            }
            ligne_droite++;
        }
        if(abs(new_bari) >= 15 && ligne_droite > 0 && ralentire == 0)
        {
            ralentire = 1;
            retiens = t_ligne.read_ms();
            t_ligne.reset();
            if(retiens > 3000) retiens = 3000;
            retiens = retiens /3;
        }
        if(ralentire) 
        {
            ligne_droite = (t_ligne.read_ms()-retiens)/4;
            ligne_droite = -100;
            if(t_ligne.read_ms()-retiens > 0)
            {
                ralentire = 0;
                ligne_droite = 0;
            }
        }
    }   
}

void init_bary()
{
    ajust_vit_vir=0;
    for(int i = 0; i < 3; i++)
    {
        bary_tab[i] = 0;
        bary_tab_bis[i] = 0;
    }
}