File content as of revision 1:8c488662e000:

#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();
   t1 = t.read();
   
   setup_lidar();
   setup_mpu9250();
   setup_direction();
   
   // Tableau d'angle
   for (int i =0 ; i < data_angles.size() ; i++){
       data_angles[i]=i;
       }
       
    voiture_deltat = t.read() - t1;
    t1 = t.read();
   
    int t_angle = 1090;
    int t_vitesse = 1480;
    
    // Initialisation de la commande des servos
    pwm_servo_direction.pulsewidth_us(t_angle);
    pwm_servo_vitesse.pulsewidth_us(t_vitesse);
    
   while (1){
        // données de la centrale inertiel
        recup_mpu9250();
        
        // 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)
        
        // data_distances = Radar;
        addRadar(Radar,data,prevData);
        
        for (int i =0; i<360; i++){
            data_distances[0]=float(Radar[0])/1000.0; // (m)
            }
            
        // L'actualisation ne se lance que si on a un tableau complet
        if (newRadar){
            myled2 = myled1;
            myled1 =! myled1;
            actualisation();
            }
            
        calculAngle(voiture_deltat);
        calculVitesse(voiture_deltat);
        avancer(voiture_deltat);
        
        
        // converversion vitesse/angle
        int t_angle = convers_angle_tempsh(angle_suivre);
        int t_vitesse = convers_vitesse_tempsh(vitesse_suivre);
            
        // commande des servos
        pwm_servo_direction.pulsewidth_us(t_angle);
        pwm_servo_vitesse.pulsewidth_us(t_vitesse);
            
        voiture_deltat = t.read() - t1;
        t1 = t.read();
       }
   
}