File content as of revision 18:6986eb7282ee:

#include "mbed.h"
#include <math.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] = {};
uint8_t tableau_en_cours = 0; //tableau en cours de remplissage
uint8_t flag_fin_tour_lidar=0;
int compteur_tours_lidar = 0;
int affiche_lidar = 0;
bool run = false;
Timer T;
bool first_update = false;


// 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);

void interrupt_bt_rx(void);


float distance(float x_1, float x_2, float y_1, float y_2)
{
    // Fonction qui renvoie la distance entre deux points (norme 2)
    float norm2;
    norm2 = sqrt((x_1 - x_2)*(x_1 - x_2) + (y_1 - y_2)*(y_1 - y_2));
    return norm2;
}

void update_direction(float* list_lidar_var, float* vecteur)
{
    float t = 0;
    //pc.printf("Update commence\n\r");
    if (first_update==true) {
        //T.start();
        first_update=false;
    } else {
        t = T.read();
        //pc.printf("%f\n\r",t);
        if(t>1) {
            pc.printf("Premier update depuis %f secondes\n\r",t);
        }
        //T.reset();
    }
    // Fonction de mise à jour de la direction
    float direction[2];
    int i;
    float list_lidar[360];
    uint8_t liste_fictifs[360];
    for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];

    //pour les essais
    //for(i=0; i<360; i++)
    //    list_lidar[i]=00;

    //list_lidar[0]=100;
    //list_lidar[90]=20.0;
    //list_lidar[270]=100.0;
    //list_lidar[45]=20.0;
    //list_lidar[315]=100;
    ///////////////////


    direction[0] = 1;
    direction[1] = 0;
    float avg_x, avg_y, sum_inv_dist;
    //list_lidar[180] = 50; // [mm], point fictif qui pousse la voiture
    //Définition des points fictifs poussant la voiture

    for (i=0; i<360; i++) {
        liste_fictifs[i] = 0;
    }
    for (i=90; i<=270; i++) {
        //for (int i=0; i<180; i++){ //test
        liste_fictifs[i] = 1;
    }
    //liste_fictifs[180] = 1;

    avg_x = 0;
    avg_y = 0;

    // Calcul de la direction à prende en fonction des charges fictives
    for (i=0; i<360; i++) {
        int theta;
        float r, x, y;
        theta = i;
        if (liste_fictifs[theta] == 1) {
            r = 500;
        } else {
            //r = 0; //test
            if(theta != 0) r = list_lidar[360-theta];
            else r = list_lidar[0];

        }

        if (r != 0) {

            //x = 0;
            //y = 0;
            x = r*cosf((float)theta*3.14/180);
            y = r*sinf((float)theta*3.14/180);
            //sum_inv_dist += 1/pow(r, 2);
            //avg_x -= x/pow(r,2);
            //avg_y -= y/pow(r,2);
            float puissance = 0.5*cosf(2*theta*3.14/180) + 1.5;
            avg_x = avg_x - x/pow(r,puissance);
            avg_y = avg_y - y/pow(r,puissance);

            //pc.printf("Angle:%i r:%4.2f x:%4.2f y:%4.2f %4.2f %4.2f\n\r", theta, r, x,y, avg_x,avg_y);
        }
    }

    //avg_x /= sum_inv_dist;
    //avg_y /= sum_inv_dist;
    direction[0] = avg_x;
    direction[1] = avg_y;
    //pc.printf("Update termine\n\r");
    // mise à jour de la direction
    for(i=0; i<2; i++)
        vecteur[i] = direction[i];

}

float angle_servo(float *direction)
{
    // Calcul basé sur la régression expérimental pour obetenir l'angle
    // le pwm à donner au moteur en fonction de l'angle voulue

    float angle;
    double pwm;
    float x, y;
    x = direction[0];
    y = direction[1];
    //x = 1;
    //y = 1;
    angle = atan2(y,x);
    angle = angle*180/3.14;
    //pc.printf("Angle : %f\n\r",angle);
    //pwm = 14.662756 * angle + 1453.08; // à refaire
    pwm = -13.30 * angle + 1376.75;

    //if (pwm < 1115) printf("trop petit\n\r");
    //if (pwm > 1625) printf("trop grand\n\r");
    //if (angle > 5*3.14/180){
    //    pwm = 1745;
    //}
    //else{
    //    if (angle < -5*3.14/180){
    //        pwm = 1080;
    //        }
    //    else{
    //        pwm = 1453;
    //        }
    //}

    return pwm;
}

void afficher_lidar(float *tableau_distances_var)
{
    //Affiche les données du lidar dans la liaison série
    int angle;
    float tableau_distances[360];
    for(angle=0; angle<360; angle++) tableau_distances[angle]=tableau_distances_var[angle];

    for(angle=0; angle<360; angle++) {
        float distance = tableau_distances[angle];
        pc.printf("%i,%f\n\r",angle,distance);
    }
}

int main()
{

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

    float dir[2]; // direction
    float pwm_direction_value;
    int pwm_mot_value = 1480;


    int i;




    // pwm du LIDAR
    pwm_lidar.period_us(40);
    pwm_lidar.pulsewidth_us(40); // vitesse fixe

    //pwm moteur
    pwm_moteur.period_ms(20);
    pwm_moteur.pulsewidth_us(pwm_mot_value);

    // 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(i=0; i<7; i++)
        lidar.getc();

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

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

    while (1) {
        //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
        //Test
        float t = T.read_us();
        pc.printf("%f\n\r",T.read_us());
        wait(2);
        if(0) {
            if(tableau_en_cours == 0)
                afficher_lidar(tableau_distance1);
            else afficher_lidar(tableau_distance0);

            affiche_lidar = 0;
        }

        if(flag_fin_tour_lidar==1) {
            flag_fin_tour_lidar=0;
            if(tableau_en_cours == 0)
                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==false && pwm_mot_value != 1480) {
            // vitesse nulle
            pwm_mot_value = 1480;
            pwm_moteur.pulsewidth_us(pwm_mot_value);
            pc.printf("pwm_mot = %i\n\r",pwm_mot_value);

        }
        if (run==true) {
            if(pwm_mot_value != 1440) {
                pwm_mot_value = 1440;
                pwm_moteur.pulsewidth_us(pwm_mot_value);
                pc.printf("pwm_mot = %i\n\r",pwm_mot_value);
            }

            pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm de la direction
        }
    }
}

void interrupt_bt_rx(void)
{
    char entree = pc.getc();
    pc.printf("%c\n\r",entree);
    if (entree == 'a') {
        run = true;
        pc.printf("c'est parti Bruno !\n\r");
    }
    if (entree == 'z') {
        run = false;
        pc.printf("Hola, tout doux !\n\r");
    }

}

void interrupt_lidar_rx(void)
{

    int SEUIL = 0; // Seuil de qualité

    static uint8_t data[5],i=0;
    uint16_t Quality;
    uint16_t Angle;
    uint16_t Distance;
    uint16_t Angle_d;
    static uint16_t Angle_d_old=0;
    uint16_t Distance_d;
    affiche_lidar ++;
    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=1-tableau_en_cours;
            flag_fin_tour_lidar=1;
        }

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

        Angle_d_old = Angle;

        //tableau_distance[Angle_d] = Distance_d;
    }
}