traitement_image.cpp

00001 #include "Mlib.h"
00002 
00003 /**********TRAITEMENT_IMG************/
00004 
00005 void F_TraitementImage()
00006 {    
00007     unsigned char i;
00008     signed short save_a;
00009     signed short save_b;
00010     
00011     int tempo = 0;
00012     char tab[3];
00013     char tab2[3];
00014     mem_a = 100;
00015     mem_b = 100;
00016     save_b = MARGE_LIGNE;
00017     save_a = MARGE_LIGNE;
00018     somme_derivee = 0;
00019      for(i = 0; i < 62-4; i++) //analyse de la partie droite de la piste
00020     {
00021         tab[0] = cam_data[AMIDTAB+i];
00022         tab[1] = cam_data[AMIDTAB+i+1];
00023         tab[2] = cam_data[AMIDTAB+i+2];
00024         tab2[0] = cam_data[AMIDTAB+i+3];
00025         tab2[1] = cam_data[AMIDTAB+i+4];
00026         tab2[2] = cam_data[AMIDTAB+i+5];
00027         tempo = middle_3(tab)-middle_3(tab2);
00028         if(tempo < 0) tempo = 0;
00029         somme_derivee+= tempo;
00030         if(save_a < tempo)
00031         {
00032             save_a = tempo;
00033             mem_a = i;
00034         }
00035     }
00036     for(i = 1; i < 63-4; i++) //analyse de la partie gauche de la piste
00037     {
00038         tab[0] = cam_data[AMIDTAB-i];
00039         tab[1] = cam_data[AMIDTAB-i-1];
00040         tab[2] = cam_data[AMIDTAB-i-2];
00041         tab2[0] = cam_data[AMIDTAB-i-3];
00042         tab2[1] = cam_data[AMIDTAB-i-4];
00043         tab2[2] = cam_data[AMIDTAB-i-5];
00044         tempo = middle_3(tab)-middle_3(tab2);
00045         if(tempo < 0) tempo = 0;
00046         somme_derivee+= tempo;
00047         if(save_b < tempo)
00048         {
00049             save_b = tempo;
00050             mem_b = i;
00051         }
00052     }
00053     
00054     Interval[0] = mem_a+mem_b;
00055     if (mem_b == 100 && mem_a == 100)  // pas de ligne
00056     {
00057         ajust_vit_vir=0;
00058         baricentre = 0;
00059     }
00060     else if (mem_b == 100)   //pas de ligne à gauche
00061     {                       //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
00062         //baricentre = (mem_a-MARGE_DISTANCE_LIGNE);
00063         ajust_vit_vir=20;
00064         if(mem_a>45){
00065             baricentre = (64-mem_a);
00066             ajust_vit_vir=0;
00067         }
00068     }
00069     else if (mem_a == 100)     // pas de ligne à droite
00070     {                          //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
00071         //baricentre = (MARGE_DISTANCE_LIGNE-mem_b);
00072         ajust_vit_vir=20;  
00073         if(mem_b>45){
00074             baricentre = (64-mem_b);
00075             ajust_vit_vir=0;
00076         }
00077         
00078     }
00079     else
00080     {
00081         ajust_vit_vir=0;
00082         baricentre = (mem_b - mem_a);    
00083         //baricentre = -baricentre;
00084     }
00085 } 
00086 
00087 void F_BalanceBlancs()
00088 {
00089     static unsigned char moy = 0;
00090     moy = Moyenne(cam_data);
00091     tempsInte+=100*(LUMI_MOYENNE-moy);
00092     if(tempsInte>MAX_INTE) tempsInte = MAX_INTE;
00093     if(tempsInte<MIN_INTE) tempsInte = MIN_INTE;
00094     somme_derivee = somme_derivee/(moy/10.0);
00095 }
00096 
00097 
00098 /*void Gestion_bosse()
00099 {
00100     if(BOSSE_ACTIVE)//Si on active la détection de la bosse
00101     {
00102         static int tep = 0;
00103         switch(bosse)
00104         {
00105             case 0:
00106             led1 = 1;
00107             led2 = 0;
00108             break;
00109             case 1:
00110             led2 = 1;
00111             led1= 0;
00112             break;
00113             case 2:
00114             led3 = 1;
00115             led2 = 0;
00116             break;
00117         }
00118     
00119         
00120         last_three[2] = last_three[1];
00121         last_three[1] = last_three[0];
00122         last_three[0] = acc.getAccX();
00123         
00124         if(bosse == 1)
00125         ajust_vitesse = 20;
00126         else if(bosse == 2)
00127         ajust_vitesse = -10;
00128         else ajust_vitesse = 0;
00129         
00130         if((middle_3F(last_three) > 0.2 ) && (bosse == 0 && t_debut.read() > 2))
00131         {
00132             bosse = 1;
00133             tep = (int)t_debut.read_ms();
00134         }
00135         else if(bosse == 1 && (int)t_debut.read_ms() > (tep+1000))
00136         {
00137             bosse = 2;
00138         }
00139         else if(bosse == 2 && middle_3F(last_three) > 0.98)
00140         {
00141             bosse = 3;
00142         }
00143     }
00144 }*/
00145 
00146 void T_image()
00147 {
00148     static int  last_bari = 0, bari_bis = 0;
00149     static int retiens = 0;
00150     F_TraitementImage();//donne une valeur au baricentre
00151     
00152     bary_tab[2] = bary_tab[1];//décalage du tableau des 3 derniers bari
00153     bary_tab[1] = bary_tab[0];
00154     bary_tab[0] = baricentre;
00155     
00156     for(int i = 0; i < 3; i++)//on rentre le tableau dans un autre tableau
00157     {
00158         bary_tab_bis[i] = bary_tab[i];
00159     }
00160     
00161     bari_bis = middle_3U(bary_tab_bis);//On calcul la médianne des 3 derniers bari
00162     integral += bari_bis;
00163     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
00164     //pc.printf("%d  ",new_bari);
00165     last_bari = bari_bis;
00166     
00167     led4 = ralentire;
00168     if(S3)//gestion des lignes droite
00169     {
00170         if(abs(new_bari) < 15 && ligne_droite < 10 && ralentire == 0)
00171         {
00172             if(ligne_droite == 0)
00173             {
00174                 t_ligne.reset();
00175             }
00176             ligne_droite++;
00177         }
00178         if(abs(new_bari) >= 15 && ligne_droite > 0 && ralentire == 0)
00179         {
00180             ralentire = 1;
00181             retiens = t_ligne.read_ms();
00182             t_ligne.reset();
00183             if(retiens > 3000) retiens = 3000;
00184             retiens = retiens /3;
00185         }
00186         if(ralentire) 
00187         {
00188             ligne_droite = (t_ligne.read_ms()-retiens)/4;
00189             ligne_droite = -100;
00190             if(t_ligne.read_ms()-retiens > 0)
00191             {
00192                 ralentire = 0;
00193                 ligne_droite = 0;
00194             }
00195         }
00196     }   
00197 }
00198 
00199 void init_bary()
00200 {
00201     ajust_vit_vir=0;
00202     for(int i = 0; i < 3; i++)
00203     {
00204         bary_tab[i] = 0;
00205         bary_tab_bis[i] = 0;
00206     }
00207 }