File content as of revision 28:c9d882501013:

#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
DigitalOut si(PTE0);  // start cameras

AnalogIn pix1(PTB2);//lecture camera1
AnalogIn pix2(PTB3);//lecture camera2
double ordre_servo=0;
#define ordre_temporel 30
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;


AnalogOut FLAG(DAC0_OUT);

void readline(void) // fonction de détection de la ligne
{
    clk=0; // la clock est nulle au départ
    int compteur = 0,index_pixel = 0, capture_finie = 0;

    while (!capture_finie) {
        if (compteur & 1)    // si compteur impair => front descendant
            clk = 0;
        else                // compteur pair => montant
            clk = 1;
        if(compteur == 5)
            si = 1;

        if (compteur == 7)
            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();
            index_pixel ++;
        }
        wait_us(10);
        compteur++;

        if (index_pixel == 128)
            capture_finie = 1;
    }
}

void passebas(int ordre)
{

    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]/=(ordre+1);
        tamponpixel1[255-i]/=(ordre+1);
        tamponpixel2[i]/=(ordre+1);
        tamponpixel2[255-i]/=(ordre+1);
    }

    // Prolongement par continuité à gauche et à droite
    for (i=0; i<ordre; i++) {
        tamponpixel1[i]=tamponpixel1[ordre];
        tamponpixel1[255-i]=tamponpixel1[255-ordre]; 
        tamponpixel2[i]=tamponpixel2[ordre];
        tamponpixel2[255-i]=tamponpixel2[255-ordre];         
    }

    // 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.;
    }
}

//fonction qui dérive le signal de la camera
void derivation()
{
    double tamponpixel1[128] = {0};
    double tamponpixel2[128] = {0};
    
    for (int i=1; i<128; i++) {
        tamponpixel1[i]=(pixel1[i]-pixel1[i-1]);
        tamponpixel2[i]=(pixel2[i]-pixel2[i-1]);
    }
    tamponpixel1[0]=tamponpixel1[1];
    tamponpixel2[0]=tamponpixel2[1];

    // Actualisation de l'image filtrée
    for (int i=0; i<128; i++) {
        pixel1[i]=tamponpixel1[i];
        pixel2[i]=tamponpixel2[i];
    }

}

void moyenne_temporelle()
{
    // Lignes de pixels passés
    double past_pixels1[128][ordre_temporel]={0};
    double 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[i];
        past_pixels2[i][0]=pixel2[i];
    }
    // Moyenne des pixels passés enregistrée sur le plus
    // vieux pixel qui sera effacé à la prochaine moyenne
    for (j=0;j<ordre_temporel;j++)
    {
        for (i=0;i<128;i++)
        {
            past_pixels1[i][ordre_temporel-1] += past_pixels1[i][j];
            past_pixels2[i][ordre_temporel-1] += past_pixels2[i][j];
        }
    }     
    // On n'oublie pas de diviser par l'ordre pour faire la moyenne
    for (i=0;i<128;i++)
    {
        pixel1[i] = past_pixels1[i][ordre_temporel-1]/ordre_temporel;
        pixel2[i] = past_pixels2[i][ordre_temporel-1]/ordre_temporel;
    }
}
void interrupt_camera()
{
    int indexMin=0; 
    int indexMax=127;
    int max_detect;
    FLAG.write(1);
    readline();
    passebas(10);
    derivation();
    moyenne_temporelle();
    //passebas(4); 

    for (int i=0;i<5;i++)
    {
        pixel1[i]=0;
        pixel2[i]=0;
        pixel1[127-i]=0;
        pixel2[127-i]=0;
    } 
    max_detect1=indexMin;
    max_detect2=indexMin;
    for (int j=indexMin; j<=indexMax; j++)
    {
        if (pixel1[j]>pixel1[max_detect1])
        {
            max_detect1=j;
        }
        if (pixel2[j]>pixel2[max_detect2])
        {
            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=8*(1-(max_detect-64)/150.);
        consigne_moteur_2=8*(1-(max_detect-64)/100.);
    }
    else
    {
        consigne_moteur_1=8*(1-(64-max_detect)/100.);
        consigne_moteur_2=8*(1-(64-max_detect)/150.); 
    }
    // Lecture du potentiometre
    //Kp_servo=2.0*pot1.read();
    ordre_servo=(/*consigne*/0-(max_detect1-max_detect2));//ya une merde ici

 
    ordre_servo=(-ordre_servo)/254+0.5;
    
    if (ordre_servo >=0.88)
        servo=0.88;
    else if (ordre_servo <=0.22)
        servo=0.22;
    else
        servo= ordre_servo; 
    flag_new_image=1;
    FLAG.write(0);
    
}
    
void init_camera()
{
    ticker_cam.attach_us(&interrupt_camera,Periode_capture_image_us); 
}