File content as of revision 5:7714a216c91d:

#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;
    //pc.printf("mem_a=%d   mem_b=%d",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
        
        ajust_vit_vir=40;
        //ralentire=1;
        if(mem_a>SEUIL_INT_VIR){
            baricentre = (64-mem_a);
            if (baricentre>25){
                ajust_vit_vir=40;
            }
            else ajust_vit_vir=20;
        }
    }
    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
        
        ajust_vit_vir=40; 
         
        if(mem_b>SEUIL_INT_VIR){
            baricentre = (64-mem_b);
            if (baricentre>25){
                ajust_vit_vir=40;
            }
            else ajust_vit_vir=20;
        }
        
    }
    else
    {
        
        baricentre = (mem_b - mem_a);  
          
        if (baricentre>20){
            ajust_vit_vir=20;
            }
        else ajust_vit_vir=0;
        
    }
} 

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 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;
    }
}