7Robot_Freescale / Mbed 2 deprecated freescal_cup_k22f

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed freescal_cup_k22f

Dependents:   freescal_cup_k22f

source/Camera.cpp@26:a836e62e0c98, 2015-01-21 (annotated)

Committer:
RobinN7
Date:
Wed Jan 21 16:54:53 2015 +0000
Revision:
26:a836e62e0c98
Parent:
25:f9d3d30cbb5d
Child:
27:d1da489fc79a
pp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobinN7 0:3af30bfbc3e5 1 #include "mbed.h"
RobinN7 26:a836e62e0c98 2 #include "QEI.h"
RobinN7 26:a836e62e0c98 3 #include "Gestion_Moteur.h"
RobinN7 0:3af30bfbc3e5 4 #include "Camera.h"
RobinN7 26:a836e62e0c98 5 #include "Servo.h"
RobinN7 0:3af30bfbc3e5 6
AlexandreN7 18:278f49df6df3 7 // les 2 cameras sont en parallèle
RobinN7 21:9430357e777c 8 DigitalOut clk(PTE1); //clock cameras
RobinN7 21:9430357e777c 9 DigitalOut si(PTE0); // start cameras
AlexandreN7 18:278f49df6df3 10
AlexandreN7 18:278f49df6df3 11 AnalogIn pix1(PTB2);//lecture camera1
AlexandreN7 18:278f49df6df3 12 AnalogIn pix2(PTB3);//lecture camera2
RobinN7 26:a836e62e0c98 13 double ordre=0;
RobinN7 26:a836e62e0c98 14 #define ordre_temporel 5
RobinN7 26:a836e62e0c98 15 int max_detect1;
RobinN7 26:a836e62e0c98 16 int max_detect2;
RobinN7 26:a836e62e0c98 17 int flag_new_image=0;
RobinN7 26:a836e62e0c98 18 double pixel1[128]= {0}; //
RobinN7 26:a836e62e0c98 19 double pixel2[128]= {0}; //
RobinN7 26:a836e62e0c98 20 Servo servo(PTD0);
RobinN7 26:a836e62e0c98 21 double Periode_capture_image_us=50000;
RobinN7 26:a836e62e0c98 22 Ticker ticker_cam;
AlexandreN7 18:278f49df6df3 23
RobinN7 26:a836e62e0c98 24 AnalogOut FLAG(DAC0_OUT);
RobinN7 0:3af30bfbc3e5 25
AlexandreN7 8:8f886cd6a59f 26 void readline(void) // fonction de détection de la ligne
RobinN7 0:3af30bfbc3e5 27 {
RobinN7 0:3af30bfbc3e5 28 clk=0; // la clock est nulle au départ
RobinN7 0:3af30bfbc3e5 29 int compteur = 0,index_pixel = 0, capture_finie = 0;
AlexandreN7 8:8f886cd6a59f 30
AlexandreN7 8:8f886cd6a59f 31 while (!capture_finie) {
RobinN7 0:3af30bfbc3e5 32 if (compteur & 1) // si compteur impair => front descendant
RobinN7 0:3af30bfbc3e5 33 clk = 0;
RobinN7 0:3af30bfbc3e5 34 else // compteur pair => montant
RobinN7 0:3af30bfbc3e5 35 clk = 1;
RobinN7 0:3af30bfbc3e5 36 if(compteur == 5)
RobinN7 0:3af30bfbc3e5 37 si = 1;
AlexandreN7 8:8f886cd6a59f 38
RobinN7 0:3af30bfbc3e5 39 if (compteur == 7)
AlexandreN7 8:8f886cd6a59f 40 si=0;
AlexandreN7 8:8f886cd6a59f 41 if ( (compteur & 1) && compteur >= 7) { // mesure sur front descendant,
RobinN7 0:3af30bfbc3e5 42 // pc.printf("lecture pixel\n");
RobinN7 26:a836e62e0c98 43 pixel1[index_pixel][0]=pix1.read_u16();
RobinN7 26:a836e62e0c98 44 pixel2[127 - index_pixel][0]=pix2.read_u16();
RobinN7 0:3af30bfbc3e5 45 index_pixel ++;
RobinN7 0:3af30bfbc3e5 46 }
RobinN7 0:3af30bfbc3e5 47 wait_us(10);
RobinN7 0:3af30bfbc3e5 48 compteur++;
AlexandreN7 8:8f886cd6a59f 49
RobinN7 0:3af30bfbc3e5 50 if (index_pixel == 128)
AlexandreN7 8:8f886cd6a59f 51 capture_finie = 1;
RobinN7 0:3af30bfbc3e5 52 }
RobinN7 0:3af30bfbc3e5 53 }
RobinN7 0:3af30bfbc3e5 54
RobinN7 15:b77dc649e4f3 55 void passebas(int ordre)
RobinN7 0:3af30bfbc3e5 56 {
AlexandreN7 10:3424e7b66671 57
RobinN7 26:a836e62e0c98 58 double tamponpixel1[256] = {0};
RobinN7 26:a836e62e0c98 59 double tamponpixel2[256] = {0};
RobinN7 15:b77dc649e4f3 60 int i=0;
RobinN7 15:b77dc649e4f3 61
RobinN7 2:804797b0b298 62 // Passe bas en partant de la gauche sur tamponpixel[0:127]
RobinN7 15:b77dc649e4f3 63 // et de la droite sur tamponpixel[128:255]
AlexandreN7 8:8f886cd6a59f 64 for (i=ordre; i<128; i++) {
RobinN7 15:b77dc649e4f3 65 for (int a=0; a<=ordre; a++) {
RobinN7 26:a836e62e0c98 66 tamponpixel1[i]+=pixel1[i-a][0];
RobinN7 26:a836e62e0c98 67 tamponpixel1[255-i]+=pixel1[127-i+a][0];
RobinN7 26:a836e62e0c98 68 tamponpixel2[i]+=pixel2[i-a][0];
RobinN7 26:a836e62e0c98 69 tamponpixel2[255-i]+=pixel2[127-i+a][0];
AlexandreN7 19:5e8260f3bdb2 70
RobinN7 0:3af30bfbc3e5 71 }
AlexandreN7 19:5e8260f3bdb2 72 tamponpixel1[i]/=(ordre+1);
AlexandreN7 19:5e8260f3bdb2 73 tamponpixel1[255-i]/=(ordre+1);
AlexandreN7 19:5e8260f3bdb2 74 tamponpixel2[i]/=(ordre+1);
AlexandreN7 19:5e8260f3bdb2 75 tamponpixel2[255-i]/=(ordre+1);
RobinN7 0:3af30bfbc3e5 76 }
AlexandreN7 8:8f886cd6a59f 77
RobinN7 15:b77dc649e4f3 78 // Prolongement par continuité à gauche et à droite
RobinN7 15:b77dc649e4f3 79 for (i=0; i<ordre; i++) {
AlexandreN7 19:5e8260f3bdb2 80 tamponpixel1[i]=tamponpixel1[ordre];
AlexandreN7 19:5e8260f3bdb2 81 tamponpixel1[255-i]=tamponpixel1[255-ordre];
AlexandreN7 19:5e8260f3bdb2 82 tamponpixel2[i]=tamponpixel2[ordre];
AlexandreN7 19:5e8260f3bdb2 83 tamponpixel2[255-i]=tamponpixel2[255-ordre];
RobinN7 0:3af30bfbc3e5 84 }
AlexandreN7 8:8f886cd6a59f 85
RobinN7 0:3af30bfbc3e5 86 // Actualisation de l'image filtrée
RobinN7 0:3af30bfbc3e5 87 for (i=0;i<128;i++)
RobinN7 0:3af30bfbc3e5 88 {
RobinN7 26:a836e62e0c98 89 pixel1[i][0]=(tamponpixel1[i]+tamponpixel1[127+i])/2;
RobinN7 26:a836e62e0c98 90 pixel2[i][0]=(tamponpixel2[i]+tamponpixel2[127+i])/2;
RobinN7 0:3af30bfbc3e5 91 }
AlexandreN7 8:8f886cd6a59f 92 }
AlexandreN7 8:8f886cd6a59f 93
AlexandreN7 8:8f886cd6a59f 94 //fonction qui dérive le signal de la camera
AlexandreN7 8:8f886cd6a59f 95 void derivation()
AlexandreN7 8:8f886cd6a59f 96 {
RobinN7 26:a836e62e0c98 97 double tamponpixel1[128] = {0};
RobinN7 26:a836e62e0c98 98 double tamponpixel2[128] = {0};
AlexandreN7 8:8f886cd6a59f 99
AlexandreN7 8:8f886cd6a59f 100 for (int i=1; i<128; i++) {
AlexandreN7 19:5e8260f3bdb2 101 tamponpixel1[i]=(pixel1[i]-pixel1[i-1]);
AlexandreN7 19:5e8260f3bdb2 102 tamponpixel2[i]=(pixel2[i]-pixel2[i-1]);
AlexandreN7 8:8f886cd6a59f 103 }
AlexandreN7 19:5e8260f3bdb2 104 tamponpixel1[0]=tamponpixel1[1];
AlexandreN7 19:5e8260f3bdb2 105 tamponpixel2[0]=tamponpixel2[1];
AlexandreN7 8:8f886cd6a59f 106
AlexandreN7 8:8f886cd6a59f 107 // Actualisation de l'image filtrée
AlexandreN7 8:8f886cd6a59f 108 for (int i=0; i<128; i++) {
AlexandreN7 19:5e8260f3bdb2 109 pixel1[i]=tamponpixel1[i];
AlexandreN7 19:5e8260f3bdb2 110 pixel2[i]=tamponpixel2[i];
AlexandreN7 8:8f886cd6a59f 111 }
AlexandreN7 8:8f886cd6a59f 112
RobinN7 25:f9d3d30cbb5d 113 }
RobinN7 26:a836e62e0c98 114
RobinN7 26:a836e62e0c98 115 void moyenne_temporelle()
RobinN7 25:f9d3d30cbb5d 116 {
RobinN7 26:a836e62e0c98 117 // Lignes de pixels passés
RobinN7 26:a836e62e0c98 118 past_pixels1[128][ordre_temporel]={0};
RobinN7 26:a836e62e0c98 119 past_pixels2[128][ordre_temporel]={0};
RobinN7 26:a836e62e0c98 120
RobinN7 26:a836e62e0c98 121 int i,j;
RobinN7 25:f9d3d30cbb5d 122
RobinN7 26:a836e62e0c98 123 // Décalage des pixels passés vers le passé
RobinN7 26:a836e62e0c98 124 for (j=ordre_temporel-1;j>0;j--)
RobinN7 26:a836e62e0c98 125 {
RobinN7 26:a836e62e0c98 126 for (i=0;i<128;i++)
RobinN7 26:a836e62e0c98 127 {
RobinN7 26:a836e62e0c98 128 past_pixels1[i][j]=past_pixels1[i][j-1];
RobinN7 26:a836e62e0c98 129 past_pixels2[i][j]=past_pixels2[i][j-1];
RobinN7 26:a836e62e0c98 130 }
RobinN7 26:a836e62e0c98 131 }
RobinN7 26:a836e62e0c98 132 // Le premier pixel passé est le pixel présent
RobinN7 26:a836e62e0c98 133 for (i=0;i<128;i++)
RobinN7 26:a836e62e0c98 134 {
RobinN7 26:a836e62e0c98 135 past_pixels1[i][0]=pixel1[]
RobinN7 26:a836e62e0c98 136 past_pixels1[i][0]=pixel2[]
RobinN7 26:a836e62e0c98 137 }
RobinN7 26:a836e62e0c98 138 // Moyenne des pixels passés enregistrée sur le plus
RobinN7 26:a836e62e0c98 139 // vieux pixel qui sera effacé à la prochaine moyenne
RobinN7 26:a836e62e0c98 140 for (i=0;i<128;i++)
RobinN7 26:a836e62e0c98 141 {
RobinN7 26:a836e62e0c98 142 for (i=0;i<128;i++)
RobinN7 26:a836e62e0c98 143 {
RobinN7 26:a836e62e0c98 144 pixel1[i][ordre2-1]=pixel1[i][ordre2-1]+pixel1[i][j];
RobinN7 26:a836e62e0c98 145 pixel2[i][ordre2-1]=pixel2[i][ordre2-1]+pixel2[i][j];
RobinN7 26:a836e62e0c98 146 }
RobinN7 26:a836e62e0c98 147 pixel1[i]=pixel1[i][0]/ordre2;
RobinN7 26:a836e62e0c98 148 pixel2[i]=pixel2[i][0]/ordre2;
RobinN7 26:a836e62e0c98 149 }
RobinN7 26:a836e62e0c98 150
RobinN7 26:a836e62e0c98 151
RobinN7 26:a836e62e0c98 152 }
RobinN7 26:a836e62e0c98 153 void interrupt_camera()
RobinN7 26:a836e62e0c98 154 {
RobinN7 26:a836e62e0c98 155 int indexMin=0;
RobinN7 26:a836e62e0c98 156 int indexMax=127;
RobinN7 26:a836e62e0c98 157 int max_detect;
RobinN7 26:a836e62e0c98 158 FLAG.write(1);
RobinN7 26:a836e62e0c98 159 readline();
RobinN7 26:a836e62e0c98 160 passebas(10);
RobinN7 26:a836e62e0c98 161 derivation();
RobinN7 26:a836e62e0c98 162 moyenne_temporel();
RobinN7 26:a836e62e0c98 163 //passebas(4);
RobinN7 26:a836e62e0c98 164
RobinN7 26:a836e62e0c98 165 for (int i=0;i<15;i++)
RobinN7 26:a836e62e0c98 166 {
RobinN7 26:a836e62e0c98 167 pixel1[i][0]=0;
RobinN7 26:a836e62e0c98 168 pixel2[i][0]=0;
RobinN7 26:a836e62e0c98 169 pixel1[127-i][0]=0;
RobinN7 26:a836e62e0c98 170 pixel2[127-i][0]=0;
RobinN7 26:a836e62e0c98 171 }
RobinN7 26:a836e62e0c98 172 max_detect1=indexMin;
RobinN7 26:a836e62e0c98 173 max_detect2=indexMin;
RobinN7 26:a836e62e0c98 174 for (int j=indexMin; j<=indexMax; j++)
RobinN7 26:a836e62e0c98 175 {
RobinN7 26:a836e62e0c98 176 if (pixel1[j][0]>pixel1[max_detect1][0])
RobinN7 26:a836e62e0c98 177 {
RobinN7 26:a836e62e0c98 178 max_detect1=j;
RobinN7 26:a836e62e0c98 179 }
RobinN7 26:a836e62e0c98 180 if (pixel2[j][0]>pixel2[max_detect2][0])
RobinN7 26:a836e62e0c98 181 {
RobinN7 26:a836e62e0c98 182 max_detect2=j;
RobinN7 26:a836e62e0c98 183 }
RobinN7 26:a836e62e0c98 184 }
RobinN7 26:a836e62e0c98 185 max_detect=(max_detect1+max_detect2)/2;
RobinN7 26:a836e62e0c98 186 // Réduction proportionelle de la vitesse moteur si l'angle du servo augmente
RobinN7 26:a836e62e0c98 187 if (max_detect>64)
RobinN7 26:a836e62e0c98 188 {
RobinN7 26:a836e62e0c98 189 consigne_moteur_1=6*(1-(max_detect-64)/150.);
RobinN7 26:a836e62e0c98 190 consigne_moteur_2=6*(1-(max_detect-64)/100.);
RobinN7 26:a836e62e0c98 191 }
RobinN7 26:a836e62e0c98 192 else
RobinN7 26:a836e62e0c98 193 {
RobinN7 26:a836e62e0c98 194 consigne_moteur_1=6*(1-(64-max_detect)/100.);
RobinN7 26:a836e62e0c98 195 consigne_moteur_2=6*(1-(64-max_detect)/150.);
RobinN7 26:a836e62e0c98 196 }
RobinN7 26:a836e62e0c98 197 // Lecture du potentiometre
RobinN7 26:a836e62e0c98 198 //Kp_servo=2.0*pot1.read();
RobinN7 26:a836e62e0c98 199 ordre=(/*consigne*/0-(max_detect1-max_detect2));//ya une merde ici
RobinN7 26:a836e62e0c98 200
RobinN7 26:a836e62e0c98 201
RobinN7 26:a836e62e0c98 202 ordre=(-ordre)/254+0.5;
RobinN7 25:f9d3d30cbb5d 203
RobinN7 26:a836e62e0c98 204 if (ordre >=0.88)
RobinN7 26:a836e62e0c98 205 servo=0.88;
RobinN7 26:a836e62e0c98 206 else if (ordre <=0.22)
RobinN7 26:a836e62e0c98 207 servo=0.22;
RobinN7 26:a836e62e0c98 208 else
RobinN7 26:a836e62e0c98 209 servo= ordre;
RobinN7 26:a836e62e0c98 210 flag_new_image=1;
RobinN7 26:a836e62e0c98 211 FLAG.write(0);
RobinN7 26:a836e62e0c98 212 }
RobinN7 26:a836e62e0c98 213
RobinN7 26:a836e62e0c98 214 void init_camera()
RobinN7 26:a836e62e0c98 215 {
RobinN7 26:a836e62e0c98 216 ticker_cam.attach_us(&interrupt_camera,Periode_capture_image_us);
RobinN7 26:a836e62e0c98 217 }
RobinN7 26:a836e62e0c98 218
RobinN7 26:a836e62e0c98 219