Diff: traitement_image.cpp

Revision:

0:3ec7fc598e48

Child:

1:000a03402c6e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/traitement_image.cpp	Thu Jan 26 07:37:45 2017 +0000
@@ -0,0 +1,194 @@
+#include "Mlib.h"
+
+/**********TRAITEMENT_IMG************/
+
+void F_TraitementImage()
+{    
+    unsigned char i;
+    signed short save_a;
+    signed short save_b;
+    int tempo = 0;
+    char mem_a, mem_b;
+    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++) 
+    {
+        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++) 
+    {
+        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;
+        }
+    }
+    
+
+    if (mem_b == 100 && mem_a == 100)
+    {
+        baricentre = 0;
+    }
+    else if (mem_b == 100)
+    {
+        //baricentre = (mem_a-MARGE_DISTANCE_LIGNE);
+        baricentre = (64-mem_a);
+    }
+    else if (mem_a == 100)
+    {
+        //baricentre = (MARGE_DISTANCE_LIGNE-mem_b);
+        baricentre = (64-mem_b);
+    }
+    else
+    {
+        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
+    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()
+{
+    for(int i = 0; i < 3; i++)
+    {
+        bary_tab[i] = 0;
+        bary_tab_bis[i] = 0;
+    }
+}
\ No newline at end of file