File content as of revision 11:e227edfced99:

#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_distance[360] = {};
int compteur_tours_lidar = 0;
int affiche_lidar = 0;
bool run = 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);


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, float* vecteur)
{
    //pc.printf("Update commence\n\r");
    // Fonction de mise à jour de la direction
    float direction[2];
        int i;
    
    //pour les essais
    for(i=0;i<360;i++)
        list_lidar[i]=100;
    ///////////////////
    
    
    direction[0] = 0;
    direction[1] = 1;
    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
    int liste_fictifs[360];
    for (int i=0; i<360; i++){
        liste_fictifs[i] = 0;
        }
    for (int i=135; i<225; i++){
    //for (int i=0; i<180; i++){ //test
        liste_fictifs[i] = 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){
            //pc.printf("Angle,%i\n\r",theta);
            r = 50;
            }
        else{
            //pc.printf("Angle,%i\n\r",theta);
            //r = 0; //test
            r = list_lidar[359-theta];
            //pc.printf("r,%f\n\r",r);
            }
        //pc.printf("Salut 1\n\r");
        //pc.printf("%f\n\r",r);
        if (r == 0) break; // non calcul en cas de distance nul (donnée non captée) 
        //pc.printf("Salut 2\n\r");
        //x = 0;
        //y = 0; 
        x = r*cosf(theta);
        y = r*sinf(theta);
        //sum_inv_dist += 1/pow(r, 2);
        //avg_x -= x/pow(r,2);
        //avg_y -= y/pow(r,2);
        float puissance = 0.5*abs(cosf(2*theta)) + 1.5;
        avg_x -= x/pow(r,puissance);
        avg_y -= y/pow(r,puissance);
        
        }
    
    //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];
    angle = atan(x/y);
    pwm = 14.662756 * angle*180/3.14 + 1453.08; // à refaire
    
    //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)
{
    //Affiche les données du lidar dans la liaison série
    int 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 i;
  




    // 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(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;
        }
        if (entree == 'z'){
            run = false;
        }
    }
    
    
    if(1){
        afficher_lidar(tableau_distance);
        
        
        affiche_lidar = 0;    
        }
    
    update_direction(tableau_distance, 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
        pwm_moteur.pulsewidth_us(1440); 
        pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
    }
    else{
        pwm_moteur.pulsewidth_us(1480);
    }

}
}

void interrupt_lidar_rx(void)
{ 

    int SEUIL = 0; // Seuil de qualité
    
    static uint8_t data[5],i=0;
    uint16_t Quality;
    uint16_t Angle;
    static uint16_t Angle_old=0;
    uint16_t Distance;
    uint16_t Angle_d;
    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_d<0) Angle_d=0;
  
        if (Quality < SEUIL) {
            // Fiabilisation des données du LIDAR naïve
            tableau_distance[Angle_d] = tableau_distance[Angle_d - 1];
            }
        else
            tableau_distance[Angle_d] = Distance_d;

        //tableau_distance[Angle_d] = Distance_d;
    }
}