File content as of revision 6:9af875ef7b30:

#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);
DigitalOut myled3(LED3);

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 (start) {  //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 (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)
                }
                actualisation();
                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);

        }
    }

}