File content as of revision 0:105ca010735c:

#include "mbed.h"
#include "Control.h"
#define Vm 15
int32_t panError,tiltError;
uint16_t blocks;
void Suiveur_Pixy(void);
void Suiveur_Moteurs(void);
Timer temps_moteur,fin_jeux;
int main()
{
    temps_moteur.start();
    temps_moteur.reset();
    fin_jeux.start();
    fin_jeux.reset();
    while(1) {
        blocks=pixy.getBlocks();
        if(blocks) {
            Suiveur_Pixy();
            Suiveur_Moteurs();
        }
        if(temps_moteur.read()>20) {
        MotD.setVit(0);
        MotD.setVit(0);
        break;
        }
    }
}

void Suiveur_Pixy(void)
{

    //panError = X_CENTER - pixy.blocks[0].x;
    tiltError = pixy.blocks[0].y - Y_CENTER;
    //panLoop.update(panError);
    tiltLoop.update(tiltError);
    pixy.setServos(500L,tiltLoop.m_pos);
}

int16_t eant=0,e;
float kp_=0.0007,kd_=0;
//float kp_=2,kd_=0;
void Suiveur_Moteurs(void)
{
    float dt=temps_moteur.read();
    temps_moteur.reset();

    uint16_t posy=tiltLoop.m_pos;
    int vit=((Vm-4)*(PIXY_RCS_MAX_POS-posy))/(350L)+4;

    uint16_t posx=pixy.blocks[0].x;
    e=posx-PIXY_CENTER_X;

    float PID=kp_*(e)+kd_*(e-eant)/dt;
    float Vd=vit-PID;
    if(Vd<0)Vd=0;
    if(Vd>50)Vd=50;
    float Vi=Vm+PID;
    if(Vi<0)Vi=0;
    if(Vi>50)Vi=50;
    // printf("VitD: %d\t VitG: %d\n\r",(int)Vd,(int)Vi);
    MotD.setVit((int)(Vd));
    MotG.setVit((int)(Vi));

    eant=e;
}