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
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?
User | Revision | Line number | New 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 |