File content as of revision 4:c393c14f4502:

#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 
Timer t;
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);

float t1;
float voiture_deltat;

int main() {
   // 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; 
    
    t1 = t.read();
    voiture_deltat = t.read() - t1;
   while (1){
        // données de la centrale inertiel
        // data_distances = Radar;
        addRadar(Radar,data,prevData);
        recup_mpu9250(voiture_deltat); // n'utilise pas voiture_deltat
        
        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){
            myled1 = !myled1 ;
            // Conversion tableau -> vector
            for (int i =0; i<360; i++){
                data_distances[i]=float(Radar[i])/1000.0; // (m)
                }
            actualisation();
            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);
        
        if (1440 <=t_vitesse <= 1480){
            myled3=0;
            }
        else{
            myled3=1;
            }
        
        // commande des servos
        pwm_servo_direction.pulsewidth_us(t_angle);
        pwm_servo_vitesse.pulsewidth_us(t_vitesse);
        
       }
   
}