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@28:c9d882501013, 2015-01-21 (annotated)

Committer:
RobinN7
Date:
Wed Jan 21 18:19:56 2015 +0000
Revision:
28:c9d882501013
Parent:
27:d1da489fc79a
Child:
29:e7f37f801c93
bite

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