Diff: source/Camera.cpp

Revision:

26:a836e62e0c98

Parent:

25:f9d3d30cbb5d

Child:

27:d1da489fc79a

--- a/source/Camera.cpp	Wed Jan 21 10:30:39 2015 +0000
+++ b/source/Camera.cpp	Wed Jan 21 16:54:53 2015 +0000
@@ -1,5 +1,8 @@
 #include "mbed.h"
+#include "QEI.h"
+#include "Gestion_Moteur.h"
 #include "Camera.h"
+#include "Servo.h"
 
 // les 2 cameras sont en parallèle
 DigitalOut clk(PTE1); //clock cameras
@@ -7,11 +10,18 @@
 
 AnalogIn pix1(PTB2);//lecture camera1
 AnalogIn pix2(PTB3);//lecture camera2
-
-
+double ordre=0;
+#define ordre_temporel 5
+int max_detect1;
+int max_detect2;
+int flag_new_image=0;
+double pixel1[128]= {0}; //
+double pixel2[128]= {0}; //
+Servo servo(PTD0);
+double Periode_capture_image_us=50000;
+Ticker ticker_cam;
 
-extern unsigned int pixel1[128]= {0},pixel1_prec[128]= {0}; //
-extern unsigned int pixel2[128]= {0},pixel2_prec[128]= {0}; //
+AnalogOut FLAG(DAC0_OUT);
 
 void readline(void) // fonction de détection de la ligne
 {
@@ -30,8 +40,8 @@
             si=0;
         if ( (compteur & 1)  && compteur >= 7) {     // mesure sur front descendant,
             //   pc.printf("lecture pixel\n");
-            pixel1[index_pixel]=pix1.read_u16();
-            pixel2[127 - index_pixel]=pix2.read_u16();
+            pixel1[index_pixel][0]=pix1.read_u16();
+            pixel2[127 - index_pixel][0]=pix2.read_u16();
             index_pixel ++;
         }
         wait_us(10);
@@ -40,24 +50,23 @@
         if (index_pixel == 128)
             capture_finie = 1;
     }
-    wait_ms(100);
 }
 
 void passebas(int ordre)
 {
 
-    unsigned int tamponpixel1[256] = {0};
-    unsigned int tamponpixel2[256] = {0};
+    double tamponpixel1[256] = {0};
+    double tamponpixel2[256] = {0};
     int i=0;
     
     // Passe bas en partant de la gauche sur tamponpixel[0:127]
     // et de la droite sur tamponpixel[128:255]
     for (i=ordre; i<128; i++) {
         for (int a=0; a<=ordre; a++) {
-            tamponpixel1[i]+=pixel1[i-a];
-            tamponpixel1[255-i]+=pixel1[127-i+a];
-            tamponpixel2[i]+=pixel2[i-a];
-            tamponpixel2[255-i]+=pixel2[127-i+a];            
+            tamponpixel1[i]+=pixel1[i-a][0];
+            tamponpixel1[255-i]+=pixel1[127-i+a][0];
+            tamponpixel2[i]+=pixel2[i-a][0];
+            tamponpixel2[255-i]+=pixel2[127-i+a][0];            
             
         }
         tamponpixel1[i]/=(ordre+1);
@@ -77,16 +86,16 @@
     // Actualisation de l'image filtrée
     for (i=0;i<128;i++)
     {
-        pixel1[i]=(tamponpixel1[i]+tamponpixel1[127+i])/2;
-        pixel2[i]=(tamponpixel2[i]+tamponpixel2[127+i])/2;
+        pixel1[i][0]=(tamponpixel1[i]+tamponpixel1[127+i])/2;
+        pixel2[i][0]=(tamponpixel2[i]+tamponpixel2[127+i])/2;
     }
 }
 
 //fonction qui dérive le signal de la camera
 void derivation()
 {
-    unsigned int tamponpixel1[128] = {0};
-    unsigned int tamponpixel2[128] = {0};
+    double tamponpixel1[128] = {0};
+    double tamponpixel2[128] = {0};
     
     for (int i=1; i<128; i++) {
         tamponpixel1[i]=(pixel1[i]-pixel1[i-1]);
@@ -102,12 +111,109 @@
     }
 
 }
-/*
-void moyenne_temporel()
+
+void moyenne_temporelle()
 {
-   pixel1_prec[128]= {0} 
+    // Lignes de pixels passés
+    past_pixels1[128][ordre_temporel]={0};
+    past_pixels2[128][ordre_temporel]={0};
+    
+    int i,j;
     
-    
+    // Décalage des pixels passés vers le passé
+    for (j=ordre_temporel-1;j>0;j--)
+    {
+        for (i=0;i<128;i++)
+        {    
+            past_pixels1[i][j]=past_pixels1[i][j-1];
+            past_pixels2[i][j]=past_pixels2[i][j-1];
+        }
+    }
+    // Le premier pixel passé est le pixel présent
+    for (i=0;i<128;i++)
+    { 
+        past_pixels1[i][0]=pixel1[]
+        past_pixels1[i][0]=pixel2[]
+    }   
+    // Moyenne des pixels passés enregistrée sur le plus
+    // vieux pixel qui sera effacé à la prochaine moyenne
+    for (i=0;i<128;i++)
+    {
+        for (i=0;i<128;i++)
+        {
+            pixel1[i][ordre2-1]=pixel1[i][ordre2-1]+pixel1[i][j];
+            pixel2[i][ordre2-1]=pixel2[i][ordre2-1]+pixel2[i][j];
+        }
+    pixel1[i]=pixel1[i][0]/ordre2;
+    pixel2[i]=pixel2[i][0]/ordre2;
+    }
+
+
+}
+void interrupt_camera()
+{
+    int indexMin=0; 
+    int indexMax=127;
+    int max_detect;
+    FLAG.write(1);
+    readline();
+    passebas(10);
+    derivation();
+    moyenne_temporel();
+    //passebas(4); 
+
+    for (int i=0;i<15;i++)
+    {
+        pixel1[i][0]=0;
+        pixel2[i][0]=0;
+        pixel1[127-i][0]=0;
+        pixel2[127-i][0]=0;
+    } 
+    max_detect1=indexMin;
+    max_detect2=indexMin;
+    for (int j=indexMin; j<=indexMax; j++)
+    {
+        if (pixel1[j][0]>pixel1[max_detect1][0])
+        {
+            max_detect1=j;
+        }
+        if (pixel2[j][0]>pixel2[max_detect2][0])
+        {
+            max_detect2=j;
+        }   
+    }  
+    max_detect=(max_detect1+max_detect2)/2; 
+    // Réduction proportionelle de la vitesse moteur si l'angle du servo augmente
+    if (max_detect>64)
+    {
+        consigne_moteur_1=6*(1-(max_detect-64)/150.);
+        consigne_moteur_2=6*(1-(max_detect-64)/100.);
+    }
+    else
+    {
+        consigne_moteur_1=6*(1-(64-max_detect)/100.);
+        consigne_moteur_2=6*(1-(64-max_detect)/150.); 
+    }
+    // Lecture du potentiometre
+    //Kp_servo=2.0*pot1.read();
+    ordre=(/*consigne*/0-(max_detect1-max_detect2));//ya une merde ici
+
+ 
+    ordre=(-ordre)/254+0.5;
     
-    }
-    */
\ No newline at end of file
+    if (ordre >=0.88)
+        servo=0.88;
+    else if (ordre <=0.22)
+        servo=0.22;
+    else
+        servo= ordre; 
+    flag_new_image=1;
+    FLAG.write(0);
+}
+    
+void init_camera()
+{
+    ticker_cam.attach_us(&interrupt_camera,Periode_capture_image_us); 
+}
+
+    
\ No newline at end of file