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@2:a37de5eb8bf3, 2017-03-17 (annotated)

Committer:
leandre
Date:
Fri Mar 17 14:18:04 2017 +0000
Revision:
2:a37de5eb8bf3
Parent:
1:000a03402c6e 
programme avec gestion des virage plus fluide

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