File content as of revision 7:20d05f0d11a2:

#include "mbed.h"
#include "algorithme.h"
#include "lidar.h"
#include "direction.h"
#include "MPU9250_setup.h"
#include "MPU9250.h"

// Définition du timer global de la voiture
// Timer

DigitalOut myled2(LED2);


Serial pc(USBTX,USBRX,115200);
float tnul = 0.0;
float t1;
float voiture_deltat;
float deltat_start;

int main()
{
    int cpt =0;
    // On setup toute la voiture
    t.start();

    setup_lidar();
    setup_mpu9250();
    setup_direction();

    // Tableau d'angle
    for (int i =0 ; i < data_angles.size() ; i++) {
        data_angles[i]=i;
    }


    int t_angle = 1090;
    int t_vitesse = 1480;

    // commande des servos
    pwm_servo_direction.pulsewidth_us(t_angle);
    pwm_servo_vitesse.pulsewidth_us(t_vitesse);

    //Signal lumineux de démarrage
    myled1=1;
    myled2=1;
    myled3=1;
    wait_ms(10);
    myled1=0;
    myled2=0;
    myled3=0;



    while (1) {
        recuperer_start(tnul);
        if (start) {
            tstart = t.read();
            deltat_start = t.read() - tstart;
            t1 = t.read();
            voiture_deltat = t.read() - t1;
        }

        while (1) {  //while(start){
            // données de la centrale inertiel
            // data_distances = Radar;
            recuperer_start(deltat_start);
            recup_mpu9250(voiture_deltat); // n'utilise pas voiture_deltat

            deltat_start = t.read() - tstart;
            voiture_deltat = t.read() - t1;
            // mise à jour l'angle de la voiture
            voiture_angle = voiture_angle + gz*voiture_deltat;
            // gz et voiture_angle a été vérifié et marche bien
            // voiture angle = angle dans le repère du circuit

            // mise à jour la vitesse de la voiture
            //voiture_vitesse = voiture_vitesse + sqrt(ax*ax+ay*ay)*voiture_deltat*g; //(m)


            // L'actualisation ne se lance que si on a un tableau complet
           // if (newData){
             //   newData = 0;
             //   addRadar(dataReady, prevData);
             //   for(int k=0; k<4; k++){
             //       prevData[k] = dataReady[k];
             //   }
            //}
            if (newRadar) {
                cpt++;
                newRadar = false;
                //myled3 = !myled3 ;
                // Conversion tableau -> vector
                for (int i =0; i<360; i++) {
                    data_distances[i]=float(Radar[i])/1000.0; // (m)
                }
                myled3=!myled3;
                actualisation();
                //myled3=0;
                if(cpt%100==0) {
                    for(int i=0; i<360; i++) {
                        pc.printf("%3d %4.3f%\n",i,data_distances[i]);
                    }
                }
                t1 = t.read();
            }

            avancer(voiture_deltat);


            // converversion vitesse/angle
            int t_angle = convers_angle_tempsh(voiture_angle_roues);

            if (voiture_angle_roues==0) {
                myled2=1;
            } else {
                myled2=0;
            }

            int t_vitesse = convers_vitesse_tempsh(voiture_vitesse);
            t_vitesse=1480;
        
            // commande des servos
            pwm_servo_direction.pulsewidth_us(t_angle);
            pwm_servo_vitesse.pulsewidth_us(t_vitesse);

        }
    }

}