Freescale_Cachan / Mbed 2 deprecated Programme_course_30Tr

programme course avec menu amélioré

Dependencies:   MMA8451Q mbed

Fork of Programme_course_vir_am_2 by Freescale_Cachan

traitement_image.cpp@4:8b0215ec788f, 2017-03-22 (annotated)

Committer:
leandre
Date:
Wed Mar 22 14:41:20 2017 +0000
Revision:
4:8b0215ec788f
Parent:
2:a37de5eb8bf3 
30 tours

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Freescale_cup 0:3ec7fc598e48 1 #include "Mlib.h"
Freescale_cup 0:3ec7fc598e48 2
Freescale_cup 0:3ec7fc598e48 3 /**********TRAITEMENT_IMG************/
Freescale_cup 0:3ec7fc598e48 4
Freescale_cup 0:3ec7fc598e48 5 void F_TraitementImage()
Freescale_cup 0:3ec7fc598e48 6 {
Freescale_cup 0:3ec7fc598e48 7 unsigned char i;
Freescale_cup 0:3ec7fc598e48 8 signed short save_a;
Freescale_cup 0:3ec7fc598e48 9 signed short save_b;
leandre 2:a37de5eb8bf3 10
Freescale_cup 0:3ec7fc598e48 11 int tempo = 0;
Freescale_cup 0:3ec7fc598e48 12 char tab[3];
Freescale_cup 0:3ec7fc598e48 13 char tab2[3];
Freescale_cup 0:3ec7fc598e48 14 mem_a = 100;
Freescale_cup 0:3ec7fc598e48 15 mem_b = 100;
Freescale_cup 0:3ec7fc598e48 16 save_b = MARGE_LIGNE;
Freescale_cup 0:3ec7fc598e48 17 save_a = MARGE_LIGNE;
Freescale_cup 0:3ec7fc598e48 18 somme_derivee = 0;
leandre 1:000a03402c6e 19 for(i = 0; i < 62-4; i++) //analyse de la partie droite de la piste
Freescale_cup 0:3ec7fc598e48 20 {
Freescale_cup 0:3ec7fc598e48 21 tab[0] = cam_data[AMIDTAB+i];
Freescale_cup 0:3ec7fc598e48 22 tab[1] = cam_data[AMIDTAB+i+1];
Freescale_cup 0:3ec7fc598e48 23 tab[2] = cam_data[AMIDTAB+i+2];
Freescale_cup 0:3ec7fc598e48 24 tab2[0] = cam_data[AMIDTAB+i+3];
Freescale_cup 0:3ec7fc598e48 25 tab2[1] = cam_data[AMIDTAB+i+4];
Freescale_cup 0:3ec7fc598e48 26 tab2[2] = cam_data[AMIDTAB+i+5];
Freescale_cup 0:3ec7fc598e48 27 tempo = middle_3(tab)-middle_3(tab2);
Freescale_cup 0:3ec7fc598e48 28 if(tempo < 0) tempo = 0;
Freescale_cup 0:3ec7fc598e48 29 somme_derivee+= tempo;
Freescale_cup 0:3ec7fc598e48 30 if(save_a < tempo)
Freescale_cup 0:3ec7fc598e48 31 {
Freescale_cup 0:3ec7fc598e48 32 save_a = tempo;
Freescale_cup 0:3ec7fc598e48 33 mem_a = i;
Freescale_cup 0:3ec7fc598e48 34 }
Freescale_cup 0:3ec7fc598e48 35 }
leandre 1:000a03402c6e 36 for(i = 1; i < 63-4; i++) //analyse de la partie gauche de la piste
Freescale_cup 0:3ec7fc598e48 37 {
Freescale_cup 0:3ec7fc598e48 38 tab[0] = cam_data[AMIDTAB-i];
Freescale_cup 0:3ec7fc598e48 39 tab[1] = cam_data[AMIDTAB-i-1];
Freescale_cup 0:3ec7fc598e48 40 tab[2] = cam_data[AMIDTAB-i-2];
Freescale_cup 0:3ec7fc598e48 41 tab2[0] = cam_data[AMIDTAB-i-3];
Freescale_cup 0:3ec7fc598e48 42 tab2[1] = cam_data[AMIDTAB-i-4];
Freescale_cup 0:3ec7fc598e48 43 tab2[2] = cam_data[AMIDTAB-i-5];
Freescale_cup 0:3ec7fc598e48 44 tempo = middle_3(tab)-middle_3(tab2);
Freescale_cup 0:3ec7fc598e48 45 if(tempo < 0) tempo = 0;
Freescale_cup 0:3ec7fc598e48 46 somme_derivee+= tempo;
Freescale_cup 0:3ec7fc598e48 47 if(save_b < tempo)
Freescale_cup 0:3ec7fc598e48 48 {
Freescale_cup 0:3ec7fc598e48 49 save_b = tempo;
Freescale_cup 0:3ec7fc598e48 50 mem_b = i;
Freescale_cup 0:3ec7fc598e48 51 }
Freescale_cup 0:3ec7fc598e48 52 }
Freescale_cup 0:3ec7fc598e48 53
leandre 2:a37de5eb8bf3 54 Interval[0] = mem_a+mem_b;
leandre 2:a37de5eb8bf3 55 if (mem_b == 100 && mem_a == 100) // pas de ligne
Freescale_cup 0:3ec7fc598e48 56 {
leandre 4:8b0215ec788f 57 ajust_vit_vir=0;
Freescale_cup 0:3ec7fc598e48 58 baricentre = 0;
Freescale_cup 0:3ec7fc598e48 59 }
leandre 2:a37de5eb8bf3 60 else if (mem_b == 100) //pas de ligne à gauche
leandre 4:8b0215ec788f 61 { //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
Freescale_cup 0:3ec7fc598e48 62 //baricentre = (mem_a-MARGE_DISTANCE_LIGNE);
leandre 4:8b0215ec788f 63 ajust_vit_vir=20;
leandre 4:8b0215ec788f 64 if(mem_a>45){
leandre 2:a37de5eb8bf3 65 baricentre = (64-mem_a);
leandre 4:8b0215ec788f 66 ajust_vit_vir=0;
leandre 2:a37de5eb8bf3 67 }
Freescale_cup 0:3ec7fc598e48 68 }
leandre 2:a37de5eb8bf3 69 else if (mem_a == 100) // pas de ligne à droite
leandre 4:8b0215ec788f 70 { //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
Freescale_cup 0:3ec7fc598e48 71 //baricentre = (MARGE_DISTANCE_LIGNE-mem_b);
leandre 4:8b0215ec788f 72 ajust_vit_vir=20;
leandre 4:8b0215ec788f 73 if(mem_b>45){
leandre 2:a37de5eb8bf3 74 baricentre = (64-mem_b);
leandre 4:8b0215ec788f 75 ajust_vit_vir=0;
leandre 2:a37de5eb8bf3 76 }
leandre 2:a37de5eb8bf3 77
Freescale_cup 0:3ec7fc598e48 78 }
Freescale_cup 0:3ec7fc598e48 79 else
Freescale_cup 0:3ec7fc598e48 80 {
leandre 4:8b0215ec788f 81 ajust_vit_vir=0;
Freescale_cup 0:3ec7fc598e48 82 baricentre = (mem_b - mem_a);
Freescale_cup 0:3ec7fc598e48 83 //baricentre = -baricentre;
Freescale_cup 0:3ec7fc598e48 84 }
Freescale_cup 0:3ec7fc598e48 85 }
Freescale_cup 0:3ec7fc598e48 86
Freescale_cup 0:3ec7fc598e48 87 void F_BalanceBlancs()
Freescale_cup 0:3ec7fc598e48 88 {
Freescale_cup 0:3ec7fc598e48 89 static unsigned char moy = 0;
Freescale_cup 0:3ec7fc598e48 90 moy = Moyenne(cam_data);
Freescale_cup 0:3ec7fc598e48 91 tempsInte+=100*(LUMI_MOYENNE-moy);
Freescale_cup 0:3ec7fc598e48 92 if(tempsInte>MAX_INTE) tempsInte = MAX_INTE;
Freescale_cup 0:3ec7fc598e48 93 if(tempsInte<MIN_INTE) tempsInte = MIN_INTE;
Freescale_cup 0:3ec7fc598e48 94 somme_derivee = somme_derivee/(moy/10.0);
Freescale_cup 0:3ec7fc598e48 95 }
Freescale_cup 0:3ec7fc598e48 96
Freescale_cup 0:3ec7fc598e48 97
leandre 4:8b0215ec788f 98 /*void Gestion_bosse()
Freescale_cup 0:3ec7fc598e48 99 {
Freescale_cup 0:3ec7fc598e48 100 if(BOSSE_ACTIVE)//Si on active la détection de la bosse
Freescale_cup 0:3ec7fc598e48 101 {
Freescale_cup 0:3ec7fc598e48 102 static int tep = 0;
Freescale_cup 0:3ec7fc598e48 103 switch(bosse)
Freescale_cup 0:3ec7fc598e48 104 {
Freescale_cup 0:3ec7fc598e48 105 case 0:
Freescale_cup 0:3ec7fc598e48 106 led1 = 1;
Freescale_cup 0:3ec7fc598e48 107 led2 = 0;
Freescale_cup 0:3ec7fc598e48 108 break;
Freescale_cup 0:3ec7fc598e48 109 case 1:
Freescale_cup 0:3ec7fc598e48 110 led2 = 1;
Freescale_cup 0:3ec7fc598e48 111 led1= 0;
Freescale_cup 0:3ec7fc598e48 112 break;
Freescale_cup 0:3ec7fc598e48 113 case 2:
Freescale_cup 0:3ec7fc598e48 114 led3 = 1;
Freescale_cup 0:3ec7fc598e48 115 led2 = 0;
Freescale_cup 0:3ec7fc598e48 116 break;
Freescale_cup 0:3ec7fc598e48 117 }
Freescale_cup 0:3ec7fc598e48 118
Freescale_cup 0:3ec7fc598e48 119
Freescale_cup 0:3ec7fc598e48 120 last_three[2] = last_three[1];
Freescale_cup 0:3ec7fc598e48 121 last_three[1] = last_three[0];
Freescale_cup 0:3ec7fc598e48 122 last_three[0] = acc.getAccX();
Freescale_cup 0:3ec7fc598e48 123
Freescale_cup 0:3ec7fc598e48 124 if(bosse == 1)
Freescale_cup 0:3ec7fc598e48 125 ajust_vitesse = 20;
Freescale_cup 0:3ec7fc598e48 126 else if(bosse == 2)
Freescale_cup 0:3ec7fc598e48 127 ajust_vitesse = -10;
Freescale_cup 0:3ec7fc598e48 128 else ajust_vitesse = 0;
Freescale_cup 0:3ec7fc598e48 129
Freescale_cup 0:3ec7fc598e48 130 if((middle_3F(last_three) > 0.2 ) && (bosse == 0 && t_debut.read() > 2))
Freescale_cup 0:3ec7fc598e48 131 {
Freescale_cup 0:3ec7fc598e48 132 bosse = 1;
Freescale_cup 0:3ec7fc598e48 133 tep = (int)t_debut.read_ms();
Freescale_cup 0:3ec7fc598e48 134 }
Freescale_cup 0:3ec7fc598e48 135 else if(bosse == 1 && (int)t_debut.read_ms() > (tep+1000))
Freescale_cup 0:3ec7fc598e48 136 {
Freescale_cup 0:3ec7fc598e48 137 bosse = 2;
Freescale_cup 0:3ec7fc598e48 138 }
Freescale_cup 0:3ec7fc598e48 139 else if(bosse == 2 && middle_3F(last_three) > 0.98)
Freescale_cup 0:3ec7fc598e48 140 {
Freescale_cup 0:3ec7fc598e48 141 bosse = 3;
Freescale_cup 0:3ec7fc598e48 142 }
Freescale_cup 0:3ec7fc598e48 143 }
leandre 4:8b0215ec788f 144 }*/
Freescale_cup 0:3ec7fc598e48 145
Freescale_cup 0:3ec7fc598e48 146 void T_image()
Freescale_cup 0:3ec7fc598e48 147 {
Freescale_cup 0:3ec7fc598e48 148 static int last_bari = 0, bari_bis = 0;
Freescale_cup 0:3ec7fc598e48 149 static int retiens = 0;
Freescale_cup 0:3ec7fc598e48 150 F_TraitementImage();//donne une valeur au baricentre
Freescale_cup 0:3ec7fc598e48 151
Freescale_cup 0:3ec7fc598e48 152 bary_tab[2] = bary_tab[1];//décalage du tableau des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 153 bary_tab[1] = bary_tab[0];
Freescale_cup 0:3ec7fc598e48 154 bary_tab[0] = baricentre;
Freescale_cup 0:3ec7fc598e48 155
Freescale_cup 0:3ec7fc598e48 156 for(int i = 0; i < 3; i++)//on rentre le tableau dans un autre tableau
Freescale_cup 0:3ec7fc598e48 157 {
Freescale_cup 0:3ec7fc598e48 158 bary_tab_bis[i] = bary_tab[i];
Freescale_cup 0:3ec7fc598e48 159 }
Freescale_cup 0:3ec7fc598e48 160
Freescale_cup 0:3ec7fc598e48 161 bari_bis = middle_3U(bary_tab_bis);//On calcul la médianne des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 162 integral += bari_bis;
Freescale_cup 0:3ec7fc598e48 163 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
leandre 2:a37de5eb8bf3 164 //pc.printf("%d ",new_bari);
Freescale_cup 0:3ec7fc598e48 165 last_bari = bari_bis;
Freescale_cup 0:3ec7fc598e48 166
Freescale_cup 0:3ec7fc598e48 167 led4 = ralentire;
Freescale_cup 0:3ec7fc598e48 168 if(S3)//gestion des lignes droite
Freescale_cup 0:3ec7fc598e48 169 {
Freescale_cup 0:3ec7fc598e48 170 if(abs(new_bari) < 15 && ligne_droite < 10 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 171 {
Freescale_cup 0:3ec7fc598e48 172 if(ligne_droite == 0)
Freescale_cup 0:3ec7fc598e48 173 {
Freescale_cup 0:3ec7fc598e48 174 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 175 }
Freescale_cup 0:3ec7fc598e48 176 ligne_droite++;
Freescale_cup 0:3ec7fc598e48 177 }
Freescale_cup 0:3ec7fc598e48 178 if(abs(new_bari) >= 15 && ligne_droite > 0 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 179 {
Freescale_cup 0:3ec7fc598e48 180 ralentire = 1;
Freescale_cup 0:3ec7fc598e48 181 retiens = t_ligne.read_ms();
Freescale_cup 0:3ec7fc598e48 182 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 183 if(retiens > 3000) retiens = 3000;
Freescale_cup 0:3ec7fc598e48 184 retiens = retiens /3;
Freescale_cup 0:3ec7fc598e48 185 }
Freescale_cup 0:3ec7fc598e48 186 if(ralentire)
Freescale_cup 0:3ec7fc598e48 187 {
Freescale_cup 0:3ec7fc598e48 188 ligne_droite = (t_ligne.read_ms()-retiens)/4;
Freescale_cup 0:3ec7fc598e48 189 ligne_droite = -100;
Freescale_cup 0:3ec7fc598e48 190 if(t_ligne.read_ms()-retiens > 0)
Freescale_cup 0:3ec7fc598e48 191 {
Freescale_cup 0:3ec7fc598e48 192 ralentire = 0;
Freescale_cup 0:3ec7fc598e48 193 ligne_droite = 0;
Freescale_cup 0:3ec7fc598e48 194 }
Freescale_cup 0:3ec7fc598e48 195 }
Freescale_cup 0:3ec7fc598e48 196 }
Freescale_cup 0:3ec7fc598e48 197 }
Freescale_cup 0:3ec7fc598e48 198
Freescale_cup 0:3ec7fc598e48 199 void init_bary()
Freescale_cup 0:3ec7fc598e48 200 {
leandre 4:8b0215ec788f 201 ajust_vit_vir=0;
Freescale_cup 0:3ec7fc598e48 202 for(int i = 0; i < 3; i++)
Freescale_cup 0:3ec7fc598e48 203 {
Freescale_cup 0:3ec7fc598e48 204 bary_tab[i] = 0;
Freescale_cup 0:3ec7fc598e48 205 bary_tab_bis[i] = 0;
Freescale_cup 0:3ec7fc598e48 206 }
Freescale_cup 0:3ec7fc598e48 207 }