Freescale_Cachan / Mbed 2 deprecated Programme_course_JourJ1

Programme course fonctionnel Jour J Croisement non pris en charge (tourne en rond)

Dependencies:   MMA8451Q mbed

Fork of Programme_course_30Tr by Freescale_Cachan

traitement_image.cpp@5:7714a216c91d, 2017-03-24 (annotated)

Committer:
leandre
Date:
Fri Mar 24 09:29:12 2017 +0000
Revision:
5:7714a216c91d
Parent:
4:8b0215ec788f 
Prog fonctionnel Jour J; Croisement ? r?gler

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 5:7714a216c91d 55 //pc.printf("mem_a=%d mem_b=%d",mem_a,mem_b);
leandre 2:a37de5eb8bf3 56 if (mem_b == 100 && mem_a == 100) // pas de ligne
Freescale_cup 0:3ec7fc598e48 57 {
leandre 4:8b0215ec788f 58 ajust_vit_vir=0;
Freescale_cup 0:3ec7fc598e48 59 baricentre = 0;
Freescale_cup 0:3ec7fc598e48 60 }
leandre 2:a37de5eb8bf3 61 else if (mem_b == 100) //pas de ligne à gauche
leandre 4:8b0215ec788f 62 { //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
leandre 5:7714a216c91d 63
leandre 5:7714a216c91d 64 ajust_vit_vir=40;
leandre 5:7714a216c91d 65 //ralentire=1;
leandre 5:7714a216c91d 66 if(mem_a>SEUIL_INT_VIR){
leandre 2:a37de5eb8bf3 67 baricentre = (64-mem_a);
leandre 5:7714a216c91d 68 if (baricentre>25){
leandre 5:7714a216c91d 69 ajust_vit_vir=40;
leandre 5:7714a216c91d 70 }
leandre 5:7714a216c91d 71 else ajust_vit_vir=20;
leandre 2:a37de5eb8bf3 72 }
Freescale_cup 0:3ec7fc598e48 73 }
leandre 2:a37de5eb8bf3 74 else if (mem_a == 100) // pas de ligne à droite
leandre 4:8b0215ec788f 75 { //si la ligne est vers le milieu on ne modifie pas le baricentre et on ralenti
leandre 5:7714a216c91d 76
leandre 5:7714a216c91d 77 ajust_vit_vir=40;
leandre 5:7714a216c91d 78
leandre 5:7714a216c91d 79 if(mem_b>SEUIL_INT_VIR){
leandre 2:a37de5eb8bf3 80 baricentre = (64-mem_b);
leandre 5:7714a216c91d 81 if (baricentre>25){
leandre 5:7714a216c91d 82 ajust_vit_vir=40;
leandre 5:7714a216c91d 83 }
leandre 5:7714a216c91d 84 else ajust_vit_vir=20;
leandre 2:a37de5eb8bf3 85 }
leandre 2:a37de5eb8bf3 86
Freescale_cup 0:3ec7fc598e48 87 }
Freescale_cup 0:3ec7fc598e48 88 else
Freescale_cup 0:3ec7fc598e48 89 {
leandre 5:7714a216c91d 90
leandre 5:7714a216c91d 91 baricentre = (mem_b - mem_a);
leandre 5:7714a216c91d 92
leandre 5:7714a216c91d 93 if (baricentre>20){
leandre 5:7714a216c91d 94 ajust_vit_vir=20;
leandre 5:7714a216c91d 95 }
leandre 5:7714a216c91d 96 else ajust_vit_vir=0;
leandre 5:7714a216c91d 97
Freescale_cup 0:3ec7fc598e48 98 }
Freescale_cup 0:3ec7fc598e48 99 }
Freescale_cup 0:3ec7fc598e48 100
Freescale_cup 0:3ec7fc598e48 101 void F_BalanceBlancs()
Freescale_cup 0:3ec7fc598e48 102 {
Freescale_cup 0:3ec7fc598e48 103 static unsigned char moy = 0;
Freescale_cup 0:3ec7fc598e48 104 moy = Moyenne(cam_data);
Freescale_cup 0:3ec7fc598e48 105 tempsInte+=100*(LUMI_MOYENNE-moy);
Freescale_cup 0:3ec7fc598e48 106 if(tempsInte>MAX_INTE) tempsInte = MAX_INTE;
Freescale_cup 0:3ec7fc598e48 107 if(tempsInte<MIN_INTE) tempsInte = MIN_INTE;
Freescale_cup 0:3ec7fc598e48 108 somme_derivee = somme_derivee/(moy/10.0);
Freescale_cup 0:3ec7fc598e48 109 }
Freescale_cup 0:3ec7fc598e48 110
Freescale_cup 0:3ec7fc598e48 111
Freescale_cup 0:3ec7fc598e48 112
Freescale_cup 0:3ec7fc598e48 113 void T_image()
Freescale_cup 0:3ec7fc598e48 114 {
Freescale_cup 0:3ec7fc598e48 115 static int last_bari = 0, bari_bis = 0;
Freescale_cup 0:3ec7fc598e48 116 static int retiens = 0;
Freescale_cup 0:3ec7fc598e48 117 F_TraitementImage();//donne une valeur au baricentre
Freescale_cup 0:3ec7fc598e48 118
Freescale_cup 0:3ec7fc598e48 119 bary_tab[2] = bary_tab[1];//décalage du tableau des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 120 bary_tab[1] = bary_tab[0];
Freescale_cup 0:3ec7fc598e48 121 bary_tab[0] = baricentre;
Freescale_cup 0:3ec7fc598e48 122
Freescale_cup 0:3ec7fc598e48 123 for(int i = 0; i < 3; i++)//on rentre le tableau dans un autre tableau
Freescale_cup 0:3ec7fc598e48 124 {
Freescale_cup 0:3ec7fc598e48 125 bary_tab_bis[i] = bary_tab[i];
Freescale_cup 0:3ec7fc598e48 126 }
Freescale_cup 0:3ec7fc598e48 127
Freescale_cup 0:3ec7fc598e48 128 bari_bis = middle_3U(bary_tab_bis);//On calcul la médianne des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 129 integral += bari_bis;
Freescale_cup 0:3ec7fc598e48 130 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 131 //pc.printf("%d ",new_bari);
Freescale_cup 0:3ec7fc598e48 132 last_bari = bari_bis;
Freescale_cup 0:3ec7fc598e48 133
leandre 5:7714a216c91d 134 //led4 = ralentire;
Freescale_cup 0:3ec7fc598e48 135 if(S3)//gestion des lignes droite
Freescale_cup 0:3ec7fc598e48 136 {
Freescale_cup 0:3ec7fc598e48 137 if(abs(new_bari) < 15 && ligne_droite < 10 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 138 {
Freescale_cup 0:3ec7fc598e48 139 if(ligne_droite == 0)
Freescale_cup 0:3ec7fc598e48 140 {
Freescale_cup 0:3ec7fc598e48 141 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 142 }
Freescale_cup 0:3ec7fc598e48 143 ligne_droite++;
Freescale_cup 0:3ec7fc598e48 144 }
Freescale_cup 0:3ec7fc598e48 145 if(abs(new_bari) >= 15 && ligne_droite > 0 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 146 {
Freescale_cup 0:3ec7fc598e48 147 ralentire = 1;
Freescale_cup 0:3ec7fc598e48 148 retiens = t_ligne.read_ms();
Freescale_cup 0:3ec7fc598e48 149 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 150 if(retiens > 3000) retiens = 3000;
Freescale_cup 0:3ec7fc598e48 151 retiens = retiens /3;
Freescale_cup 0:3ec7fc598e48 152 }
Freescale_cup 0:3ec7fc598e48 153 if(ralentire)
Freescale_cup 0:3ec7fc598e48 154 {
Freescale_cup 0:3ec7fc598e48 155 ligne_droite = (t_ligne.read_ms()-retiens)/4;
Freescale_cup 0:3ec7fc598e48 156 ligne_droite = -100;
Freescale_cup 0:3ec7fc598e48 157 if(t_ligne.read_ms()-retiens > 0)
Freescale_cup 0:3ec7fc598e48 158 {
Freescale_cup 0:3ec7fc598e48 159 ralentire = 0;
Freescale_cup 0:3ec7fc598e48 160 ligne_droite = 0;
Freescale_cup 0:3ec7fc598e48 161 }
Freescale_cup 0:3ec7fc598e48 162 }
Freescale_cup 0:3ec7fc598e48 163 }
Freescale_cup 0:3ec7fc598e48 164 }
Freescale_cup 0:3ec7fc598e48 165
Freescale_cup 0:3ec7fc598e48 166 void init_bary()
Freescale_cup 0:3ec7fc598e48 167 {
leandre 4:8b0215ec788f 168 ajust_vit_vir=0;
Freescale_cup 0:3ec7fc598e48 169 for(int i = 0; i < 3; i++)
Freescale_cup 0:3ec7fc598e48 170 {
Freescale_cup 0:3ec7fc598e48 171 bary_tab[i] = 0;
Freescale_cup 0:3ec7fc598e48 172 bary_tab_bis[i] = 0;
Freescale_cup 0:3ec7fc598e48 173 }
Freescale_cup 0:3ec7fc598e48 174 }