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