File content as of revision 18:daba0b3777c0:

#include "mbed.h"
#include <math.h>
#include "SaphTeamRacing.h"

// Définition des ports séries
//Serial pc(USBTX, USBRX, 115200);
Serial pc(PC_10, PC_11, 115200);
Serial lidar(PC_6, PC_7, 115200);

// Définition des variables globales
float tableau_distance0[360] = {};
float tableau_distance1[360] = {};
bool tableau_en_cours = false; //tableau en cours de remplissage
uint8_t flag_fin_tour_lidar=0;
int compteur_tours_lidar = 0;
bool run = false;
bool stop = true;

// Défintion des pwm
PwmOut pwm_lidar(PB_15); // pwm du Lidar
PwmOut pwm_moteur(PE_6); // pwm de la propulsion
PwmOut pwm_direction(PE_5); // pwm de la direction

void interrupt_lidar_rx(void);

int main()
{

    pc.printf("\r-------------------------\n\r");

    float dir[2]; // direction
    float pwm_direction_value;
    
    // pwm du LIDAR
    pwm_lidar.period_us(40);
    pwm_lidar.pulsewidth_us(40); // vitesse fixe

    //pwm moteur
    pwm_moteur.period_ms(20);

    // pwm de la direction
    pwm_direction.period_ms(20);
    pwm_direction.pulsewidth_us(1480); // correspond à un vitesse faible

    // récupération du premier batch de données (7 bytes) du LIDAR
    lidar.putc(0xA5);
    lidar.putc(0x20);
    for(int i=0; i<7; i++)
        lidar.getc();

    pc.printf("FIN intit \n\r");

    lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);

    while (1) {
        //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);

        if(pc.readable()) {
            char entree = pc.getc();
            pc.printf("%c \n\r",entree);
            if (entree == 'a') {
                run = true;
                stop = false;
            }
            if (entree == 'z') {
                run = false;
            }
        }


        if(0) {
            if(!tableau_en_cours)
                afficher_lidar(tableau_distance1, pc);
            else afficher_lidar(tableau_distance0, pc);

        }

        if(flag_fin_tour_lidar==1) {
            flag_fin_tour_lidar=0;
            if(!tableau_en_cours)
                update_direction(tableau_distance1, dir); // mise à jour à la direction
            else update_direction(tableau_distance0, dir); // mise à jour à la direction
            pc.printf("direction,%f,%f\n\r",dir[0],dir[1]);
            pwm_direction_value = angle_servo(dir); // calcul du pwm
        }

        if (run==true) {
            // vitesse constante
            if (stop==false) {
                pwm_moteur.pulsewidth_us(1440);
                stop=true;
            }
            pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
        } else {
            stop=false;
            pwm_moteur.pulsewidth_us(1480);
        }

    }
}

void interrupt_lidar_rx(void)
{

    int SEUIL = 0; // Seuil de qualité

    static uint8_t data[5], i = 0, Angle_d_old = 0;
    uint16_t Quality, Angle, Distance, Angle_d, Distance_d;
    data[i] = lidar.getc();
    i++;
    if(i==5) {
        i=0;
        Quality = data[0] & 0xFC;
        Quality = Quality >> 2;

        Angle = data[1] & 0xFE;
        Angle = (Angle>>1) | ((uint16_t)data[2] << 7);

        Distance = data[3];
        Distance = Distance | ((uint16_t)data[4] << 8);

        Angle_d = Angle/64; // in degree
        Distance_d = Distance>>2; // in mm

        // On vérifie que l'on écrit pas en dehors du tableau
        //Angle_d = 360 - Angle_d;
        if(Angle_d>359) Angle_d=359;

        if(Angle < (Angle_d_old - 180)) {
            tableau_en_cours=!tableau_en_cours;
            flag_fin_tour_lidar=1;
        }

        if (Quality < SEUIL) {
            // Fiabilisation des données du LIDAR naïve
            if(!tableau_en_cours)
                tableau_distance0[Angle_d] = tableau_distance0[Angle_d_old];
            else tableau_distance1[Angle_d] = tableau_distance1[Angle_d_old];
        } else if(!tableau_en_cours)
            tableau_distance0[Angle_d] = Distance_d;
        else tableau_distance1[Angle_d] = Distance_d;

        Angle_d_old = Angle;

        //tableau_distance[Angle_d] = Distance_d;
    }
}