Freescale_Cachan / Mbed 2 deprecated Programme_course_base

programme ines avce xbee

Dependencies:   MMA8451Q mbed xbee_lib

Fork of Programme_course by Freescale_Cachan

traitement_image.cpp@2:ddf9879f0e25, 2017-03-17 (annotated)

Committer:
inesmas3
Date:
Fri Mar 17 14:18:13 2017 +0000
Revision:
2:ddf9879f0e25
Parent:
1:4a9196bcf97a 
Programme ines avce le xbee

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];
inesmas3 1:4a9196bcf97a 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 }
inesmas3 2:ddf9879f0e25 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 T_image()
Freescale_cup 0:3ec7fc598e48 88 {
Freescale_cup 0:3ec7fc598e48 89 static int last_bari = 0, bari_bis = 0;
Freescale_cup 0:3ec7fc598e48 90 static int retiens = 0;
Freescale_cup 0:3ec7fc598e48 91 F_TraitementImage();//donne une valeur au baricentre
Freescale_cup 0:3ec7fc598e48 92
Freescale_cup 0:3ec7fc598e48 93 bary_tab[2] = bary_tab[1];//décalage du tableau des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 94 bary_tab[1] = bary_tab[0];
Freescale_cup 0:3ec7fc598e48 95 bary_tab[0] = baricentre;
Freescale_cup 0:3ec7fc598e48 96
Freescale_cup 0:3ec7fc598e48 97 for(int i = 0; i < 3; i++)//on rentre le tableau dans un autre tableau
Freescale_cup 0:3ec7fc598e48 98 {
Freescale_cup 0:3ec7fc598e48 99 bary_tab_bis[i] = bary_tab[i];
Freescale_cup 0:3ec7fc598e48 100 }
Freescale_cup 0:3ec7fc598e48 101
Freescale_cup 0:3ec7fc598e48 102 bari_bis = middle_3U(bary_tab_bis);//On calcul la médianne des 3 derniers bari
Freescale_cup 0:3ec7fc598e48 103 integral += bari_bis;
Freescale_cup 0:3ec7fc598e48 104 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 105 last_bari = bari_bis;
Freescale_cup 0:3ec7fc598e48 106
Freescale_cup 0:3ec7fc598e48 107 led4 = ralentire;
inesmas3 2:ddf9879f0e25 108 if(S3)//gestion des lignes droites
Freescale_cup 0:3ec7fc598e48 109 {
Freescale_cup 0:3ec7fc598e48 110 if(abs(new_bari) < 15 && ligne_droite < 10 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 111 {
Freescale_cup 0:3ec7fc598e48 112 if(ligne_droite == 0)
Freescale_cup 0:3ec7fc598e48 113 {
Freescale_cup 0:3ec7fc598e48 114 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 115 }
Freescale_cup 0:3ec7fc598e48 116 ligne_droite++;
Freescale_cup 0:3ec7fc598e48 117 }
Freescale_cup 0:3ec7fc598e48 118 if(abs(new_bari) >= 15 && ligne_droite > 0 && ralentire == 0)
Freescale_cup 0:3ec7fc598e48 119 {
inesmas3 1:4a9196bcf97a 120 ralentire = 1; // ralentire????????
Freescale_cup 0:3ec7fc598e48 121 retiens = t_ligne.read_ms();
Freescale_cup 0:3ec7fc598e48 122 t_ligne.reset();
Freescale_cup 0:3ec7fc598e48 123 if(retiens > 3000) retiens = 3000;
Freescale_cup 0:3ec7fc598e48 124 retiens = retiens /3;
Freescale_cup 0:3ec7fc598e48 125 }
Freescale_cup 0:3ec7fc598e48 126 if(ralentire)
Freescale_cup 0:3ec7fc598e48 127 {
Freescale_cup 0:3ec7fc598e48 128 ligne_droite = (t_ligne.read_ms()-retiens)/4;
Freescale_cup 0:3ec7fc598e48 129 ligne_droite = -100;
Freescale_cup 0:3ec7fc598e48 130 if(t_ligne.read_ms()-retiens > 0)
Freescale_cup 0:3ec7fc598e48 131 {
Freescale_cup 0:3ec7fc598e48 132 ralentire = 0;
Freescale_cup 0:3ec7fc598e48 133 ligne_droite = 0;
Freescale_cup 0:3ec7fc598e48 134 }
Freescale_cup 0:3ec7fc598e48 135 }
Freescale_cup 0:3ec7fc598e48 136 }
Freescale_cup 0:3ec7fc598e48 137 }
Freescale_cup 0:3ec7fc598e48 138
Freescale_cup 0:3ec7fc598e48 139 void init_bary()
Freescale_cup 0:3ec7fc598e48 140 {
Freescale_cup 0:3ec7fc598e48 141 for(int i = 0; i < 3; i++)
Freescale_cup 0:3ec7fc598e48 142 {
Freescale_cup 0:3ec7fc598e48 143 bary_tab[i] = 0;
Freescale_cup 0:3ec7fc598e48 144 bary_tab_bis[i] = 0;
Freescale_cup 0:3ec7fc598e48 145 }
Freescale_cup 0:3ec7fc598e48 146 }