7Robot_Freescale / Mbed 2 deprecated freescal_cup_k22f

Dependencies:   mbed freescal_cup_k22f

Dependents:   freescal_cup_k22f

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?

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
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