Diff: main.cpp

Revision:

2:b2ce001ff8f5

Parent:

1:e4b5a39729d2

Child:

3:46ea1b20397d

diff -r e4b5a39729d2 -r b2ce001ff8f5 main.cpp
--- a/main.cpp	Fri May 10 09:29:12 2019 +0000
+++ b/main.cpp	Fri May 10 10:07:11 2019 +0000
@@ -1,24 +1,174 @@
 #include "mbed.h"
-#include "math.h"
+#include <math.h>
+
+// Définition des ports séries
+Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+Serial lidar(PC_6, PC_7, 115200);
+
+// Définition des variables globales
+int tableau_distance[360] = {};
+int compteur_tours_lidar = 0;
+
+// 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);
 
-PwmOut pwm_servo(PE_5);
-PwmOut pwm_moteur(PE_6);
-
-Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+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(int* list_lidar, float* vecteur)
+{
+    // Fonction de mise à jour de la direction
+    float direction[2];
+    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
+    int i;
+    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;
+        r = list_lidar[theta];
+        
+        if (r == 0) break; // non calcul en cas de distance nul (donnée non capté) 
+        
+        //x = 0;
+        //y = 0; 
+        x = r*cosf(theta);
+        y = r*sinf(theta);
+        sum_inv_dist += 1/pow(r, 2);
+        avg_x -= x/sum_inv_dist;
+        avg_y -= y/sum_inv_dist;
+        }
+    
+    avg_x /= sum_inv_dist;
+    avg_y /= sum_inv_dist;
+    direction[0] = avg_x;
+    direction[1] = avg_y;
+    
+    // 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 - 1453.08; // à refaire
+    
+    if (pwm < 1115) printf("trop petit\n\r");
+    if (pwm > 1625) printf("trop grand\n\r");
+    
+    return pwm;
+}
 
 int main(){
-    // Test du bon fonctionnement de la liaison série
-    pc.printf("Start of the program \n\r");
-    pc.printf("-------------------- \n\r");  
-     
-    // Initialisation du moteur
+    
+    pc.printf("\r-------------------------\n\r");
+    
+    float dir[2]; // direction
+    float pwm_direction_value;
+    
+    pc.printf("%f", cos(3.141592/4));
+    
+    int i;
+ 
+    // pwm du LIDAR
+    pwm_lidar.period_us(40);
+    pwm_lidar.pulsewidth_us(22); // vitesse fixe
+    
+    // pwm du Moteur
     pwm_moteur.period_ms(20);
-    pwm_moteur.pulsewidth_us(1460);
+    pwm_moteur.pulsewidth_us(1470); // correspond à une vitesse nulle
+    // Gaspard : 1450, Solal : 1480. Tester les deux
+
+    // pwm de la direction
+    pwm_direction.period_ms(20);
+    pwm_direction.pulsewidth_us(1400); // 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);
+        
+    update_direction(tableau_distance, dir); // mise à jour à la direction
+    pwm_direction_value = angle_servo(dir); // calcul du pwm
     
-    // Intilisation du servo
-    pwm_servo.period_ms(20);
-    pwm_servo.pulsewidth_us(1115);
+    pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
+    }
+
+}
+
+
+void interrupt_lidar_rx(void)
+{ 
+
+    int SEUIL = 15; // Seuil de qualité
     
-    pc.printf("cos(3) = %f", cosf(3));
-    
-    }
\ No newline at end of file
+    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;
+    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 
+        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;
+
+    }
+}