Freescale_Cachan / Mbed 2 deprecated Programme_course_vir_am_2

programme course avec menu amélioré

Dependencies:   MMA8451Q mbed

Fork of Programme_course_2017_virage_ameliore by Freescale_Cachan

traitement_image.cpp@0:3ec7fc598e48, 2017-01-26 (annotated)

Committer:
Freescale_cup
Date:
Thu Jan 26 07:37:45 2017 +0000
Revision:
0:3ec7fc598e48
Child:
1:000a03402c6e
Programme de base

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;
Freescale_cup 0:3ec7fc598e48 10 int tempo = 0;
Freescale_cup 0:3ec7fc598e48 11 char mem_a, mem_b;
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;
Freescale_cup 0:3ec7fc598e48 19 for(i = 0; i < 62-4; i++)
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 }
Freescale_cup 0:3ec7fc598e48 36 for(i = 1; i < 63-4; i++)
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
Freescale_cup 0:3ec7fc598e48 54
Freescale_cup 0:3ec7fc598e48 55 if (mem_b == 100 && mem_a == 100)
Freescale_cup 0:3ec7fc598e48 56 {
Freescale_cup 0:3ec7fc598e48 57 baricentre = 0;
Freescale_cup 0:3ec7fc598e48 58 }
Freescale_cup 0:3ec7fc598e48 59 else if (mem_b == 100)
Freescale_cup 0:3ec7fc598e48 60 {
Freescale_cup 0:3ec7fc598e48 61 //baricentre = (mem_a-MARGE_DISTANCE_LIGNE);
Freescale_cup 0:3ec7fc598e48 62 baricentre = (64-mem_a);
Freescale_cup 0:3ec7fc598e48 63 }
Freescale_cup 0:3ec7fc598e48 64 else if (mem_a == 100)
Freescale_cup 0:3ec7fc598e48 65 {
Freescale_cup 0:3ec7fc598e48 66 //baricentre = (MARGE_DISTANCE_LIGNE-mem_b);
Freescale_cup 0:3ec7fc598e48 67 baricentre = (64-mem_b);
Freescale_cup 0:3ec7fc598e48 68 }
Freescale_cup 0:3ec7fc598e48 69 else
Freescale_cup 0:3ec7fc598e48 70 {
Freescale_cup 0:3ec7fc598e48 71 baricentre = (mem_b - mem_a);
Freescale_cup 0:3ec7fc598e48 72 //baricentre = -baricentre;
Freescale_cup 0:3ec7fc598e48 73 }
Freescale_cup 0:3ec7fc598e48 74 }
Freescale_cup 0:3ec7fc598e48 75
Freescale_cup 0:3ec7fc598e48 76 void F_BalanceBlancs()
Freescale_cup 0:3ec7fc598e48 77 {
Freescale_cup 0:3ec7fc598e48 78 static unsigned char moy = 0;
Freescale_cup 0:3ec7fc598e48 79 moy = Moyenne(cam_data);
Freescale_cup 0:3ec7fc598e48 80 tempsInte+=100*(LUMI_MOYENNE-moy);
Freescale_cup 0:3ec7fc598e48 81 if(tempsInte>MAX_INTE) tempsInte = MAX_INTE;
Freescale_cup 0:3ec7fc598e48 82 if(tempsInte<MIN_INTE) tempsInte = MIN_INTE;
Freescale_cup 0:3ec7fc598e48 83 somme_derivee = somme_derivee/(moy/10.0);
Freescale_cup 0:3ec7fc598e48 84 }
Freescale_cup 0:3ec7fc598e48 85
Freescale_cup 0:3ec7fc598e48 86
Freescale_cup 0:3ec7fc598e48 87 void Gestion_bosse()
Freescale_cup 0:3ec7fc598e48 88 {
Freescale_cup 0:3ec7fc598e48 89 if(BOSSE_ACTIVE)//Si on active la détection de la bosse
Freescale_cup 0:3ec7fc598e48 90 {
Freescale_cup 0:3ec7fc598e48 91 static int tep = 0;
Freescale_cup 0:3ec7fc598e48 92 switch(bosse)
Freescale_cup 0:3ec7fc598e48 93 {
Freescale_cup 0:3ec7fc598e48 94 case 0:
Freescale_cup 0:3ec7fc598e48 95 led1 = 1;
Freescale_cup 0:3ec7fc598e48 96 led2 = 0;
Freescale_cup 0:3ec7fc598e48 97 break;
Freescale_cup 0:3ec7fc598e48 98 case 1:
Freescale_cup 0:3ec7fc598e48 99 led2 = 1;
Freescale_cup 0:3ec7fc598e48 100 led1= 0;
Freescale_cup 0:3ec7fc598e48 101 break;
Freescale_cup 0:3ec7fc598e48 102 case 2:
Freescale_cup 0:3ec7fc598e48 103 led3 = 1;
Freescale_cup 0:3ec7fc598e48 104 led2 = 0;
Freescale_cup 0:3ec7fc598e48 105 break;
Freescale_cup 0:3ec7fc598e48 106 }
Freescale_cup 0:3ec7fc598e48 107
Freescale_cup 0:3ec7fc598e48 108
Freescale_cup 0:3ec7fc598e48 109 last_three[2] = last_three[1];
Freescale_cup 0:3ec7fc598e48 110 last_three[1] = last_three[0];
Freescale_cup 0:3ec7fc598e48 111 last_three[0] = acc.getAccX();
Freescale_cup 0:3ec7fc598e48 112
Freescale_cup 0:3ec7fc598e48 113 if(bosse == 1)
Freescale_cup 0:3ec7fc598e48 114 ajust_vitesse = 20;
Freescale_cup 0:3ec7fc598e48 115 else if(bosse == 2)
Freescale_cup 0:3ec7fc598e48 116 ajust_vitesse = -10;
Freescale_cup 0:3ec7fc598e48 117 else ajust_vitesse = 0;
Freescale_cup 0:3ec7fc598e48 118
Freescale_cup 0:3ec7fc598e48 119 if((middle_3F(last_three) > 0.2 ) && (bosse == 0 && t_debut.read() > 2))
Freescale_cup 0:3ec7fc598e48 120 {
Freescale_cup 0:3ec7fc598e48 121 bosse = 1;
Freescale_cup 0:3ec7fc598e48 122 tep = (int)t_debut.read_ms();
Freescale_cup 0:3ec7fc598e48 123 }
Freescale_cup 0:3ec7fc598e48 124 else if(bosse == 1 && (int)t_debut.read_ms() > (tep+1000))
Freescale_cup 0:3ec7fc598e48 125 {
Freescale_cup 0:3ec7fc598e48 126 bosse = 2;
Freescale_cup 0:3ec7fc598e48 127 }
Freescale_cup 0:3ec7fc598e48 128 else if(bosse == 2 && middle_3F(last_three) > 0.98)
Freescale_cup 0:3ec7fc598e48 129 {
Freescale_cup 0:3ec7fc598e48 130 bosse = 3;
Freescale_cup 0:3ec7fc598e48 131 }
Freescale_cup 0:3ec7fc598e48 132 }
Freescale_cup 0:3ec7fc598e48 133 }
Freescale_cup 0:3ec7fc598e48 134
Freescale_cup 0:3ec7fc598e48 135 void T_image()
Freescale_cup 0:3ec7fc598e48 136 {
Freescale_cup 0:3ec7fc598e48 137 static int last_bari = 0, bari_bis = 0;
Freescale_cup 0:3ec7fc598e48 138 static int retiens = 0;
Freescale_cup 0:3ec7fc598e48 139 F_TraitementImage();//donne une valeur au baricentre
Freescale_cup 0:3ec7fc598e48 140
Freescale_cup 0:3ec7fc598e48 141 bary_tab[2] = bary_tab[1];//décalage du tableau des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 142 bary_tab[1] = bary_tab[0];
Freescale_cup 0:3ec7fc598e48 143 bary_tab[0] = baricentre;
Freescale_cup 0:3ec7fc598e48 144
Freescale_cup 0:3ec7fc598e48 145 for(int i = 0; i < 3; i++)//on rentre le tableau dans un autre tableau
Freescale_cup 0:3ec7fc598e48 146 {
Freescale_cup 0:3ec7fc598e48 147 bary_tab_bis[i] = bary_tab[i];
Freescale_cup 0:3ec7fc598e48 148 }
Freescale_cup 0:3ec7fc598e48 149
Freescale_cup 0:3ec7fc598e48 150 bari_bis = middle_3U(bary_tab_bis);//On calcul la médianne des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 151 integral += bari_bis;
Freescale_cup 0:3ec7fc598e48 152 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
Freescale_cup 0:3ec7fc598e48 153 last_bari = bari_bis;
Freescale_cup 0:3ec7fc598e48 154
Freescale_cup 0:3ec7fc598e48 155 led4 = ralentire;
Freescale_cup 0:3ec7fc598e48 156 if(S3)//gestion des lignes droite
Freescale_cup 0:3ec7fc598e48 157 {
Freescale_cup 0:3ec7fc598e48 158 if(abs(new_bari) < 15 && ligne_droite < 10 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 159 {
Freescale_cup 0:3ec7fc598e48 160 if(ligne_droite == 0)
Freescale_cup 0:3ec7fc598e48 161 {
Freescale_cup 0:3ec7fc598e48 162 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 163 }
Freescale_cup 0:3ec7fc598e48 164 ligne_droite++;
Freescale_cup 0:3ec7fc598e48 165 }
Freescale_cup 0:3ec7fc598e48 166 if(abs(new_bari) >= 15 && ligne_droite > 0 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 167 {
Freescale_cup 0:3ec7fc598e48 168 ralentire = 1;
Freescale_cup 0:3ec7fc598e48 169 retiens = t_ligne.read_ms();
Freescale_cup 0:3ec7fc598e48 170 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 171 if(retiens > 3000) retiens = 3000;
Freescale_cup 0:3ec7fc598e48 172 retiens = retiens /3;
Freescale_cup 0:3ec7fc598e48 173 }
Freescale_cup 0:3ec7fc598e48 174 if(ralentire)
Freescale_cup 0:3ec7fc598e48 175 {
Freescale_cup 0:3ec7fc598e48 176 ligne_droite = (t_ligne.read_ms()-retiens)/4;
Freescale_cup 0:3ec7fc598e48 177 ligne_droite = -100;
Freescale_cup 0:3ec7fc598e48 178 if(t_ligne.read_ms()-retiens > 0)
Freescale_cup 0:3ec7fc598e48 179 {
Freescale_cup 0:3ec7fc598e48 180 ralentire = 0;
Freescale_cup 0:3ec7fc598e48 181 ligne_droite = 0;
Freescale_cup 0:3ec7fc598e48 182 }
Freescale_cup 0:3ec7fc598e48 183 }
Freescale_cup 0:3ec7fc598e48 184 }
Freescale_cup 0:3ec7fc598e48 185 }
Freescale_cup 0:3ec7fc598e48 186
Freescale_cup 0:3ec7fc598e48 187 void init_bary()
Freescale_cup 0:3ec7fc598e48 188 {
Freescale_cup 0:3ec7fc598e48 189 for(int i = 0; i < 3; i++)
Freescale_cup 0:3ec7fc598e48 190 {
Freescale_cup 0:3ec7fc598e48 191 bary_tab[i] = 0;
Freescale_cup 0:3ec7fc598e48 192 bary_tab_bis[i] = 0;
Freescale_cup 0:3ec7fc598e48 193 }
Freescale_cup 0:3ec7fc598e48 194 }