Diff: main.cpp

Revision:

5:73aac5fe9696

Parent:

4:c393c14f4502

Child:

6:9af875ef7b30

--- a/main.cpp	Wed Jun 05 22:55:03 2019 +0000
+++ b/main.cpp	Thu Jun 06 14:04:56 2019 +0000
@@ -6,36 +6,40 @@
 #include "MPU9250.h"
 
 // Définition du timer global de la voiture
-// Timer 
-Timer t;
+// Timer
+
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
-
+Serial pc(USBTX,USBRX,115200);
+float tnul = 0.0;
 float t1;
 float voiture_deltat;
+float deltat_start;
 
-int main() {
-   // On setup toute la voiture
-   t.start();
-   
-   setup_lidar();
-   setup_mpu9250();
-   setup_direction();
-   
-   // Tableau d'angle
-   for (int i =0 ; i < data_angles.size() ; i++){
-       data_angles[i]=i;
-       }
-    
-   
+int main()
+{
+    int cpt =0;
+    // On setup toute la voiture
+    t.start();
+
+    setup_lidar();
+    setup_mpu9250();
+    setup_direction();
+
+    // Tableau d'angle
+    for (int i =0 ; i < data_angles.size() ; i++) {
+        data_angles[i]=i;
+    }
+
+
     int t_angle = 1090;
     int t_vitesse = 1480;
-    
+
     // commande des servos
     pwm_servo_direction.pulsewidth_us(t_angle);
     pwm_servo_vitesse.pulsewidth_us(t_vitesse);
-    
+
     //Signal lumineux de démarrage
     myled1=1;
     myled2=1;
@@ -43,63 +47,81 @@
     wait_ms(10);
     myled1=0;
     myled2=0;
-    myled3=0; 
-    
-    t1 = t.read();
-    voiture_deltat = t.read() - t1;
-   while (1){
-        // données de la centrale inertiel
-        // data_distances = Radar;
-        addRadar(Radar,data,prevData);
-        recup_mpu9250(voiture_deltat); // n'utilise pas voiture_deltat
-        
-        voiture_deltat = t.read() - t1;
-        // 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)
-            
-        
-        // L'actualisation ne se lance que si on a un tableau complet
-        if (newRadar){
-            myled1 = !myled1 ;
-            // Conversion tableau -> vector
-            for (int i =0; i<360; i++){
-                data_distances[i]=float(Radar[i])/1000.0; // (m)
+    myled3=0;
+
+
+
+    while (1) {
+        recuperer_start(tnul);
+        if (start) {
+            tstart = t.read();
+            deltat_start = t.read() - tstart;
+            t1 = t.read();
+            voiture_deltat = t.read() - t1;
+        }
+
+        while (1) {  //while(start){
+            // données de la centrale inertiel
+            // data_distances = Radar;
+            recuperer_start(deltat_start);
+            recup_mpu9250(voiture_deltat); // n'utilise pas voiture_deltat
+
+            deltat_start = t.read() - tstart;
+            voiture_deltat = t.read() - t1;
+            // 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)
+
+
+            // L'actualisation ne se lance que si on a un tableau complet
+
+            if (newRadar) {
+                cpt++;
+                newRadar = false;
+                myled1 = !myled1 ;
+                // Conversion tableau -> vector
+                for (int i =0; i<360; i++) {
+                    data_distances[i]=float(Radar[i])/1000.0; // (m)
                 }
-            actualisation();
-            t1 = t.read();
+                actualisation();
+                if(cpt%100==0) {
+                    myled3 = !myled3;
+                    for(int i=0; i<360; i++) {
+                        pc.printf("%3d %4.3f%\n",i,data_distances[i]);
+                    }
+                }
+                t1 = t.read();
             }
-            
-        avancer(voiture_deltat);
-        
-        
-        // converversion vitesse/angle
-        int t_angle = convers_angle_tempsh(voiture_angle_roues);
-        
-        if (voiture_angle_roues==0){
-            myled2=1;
-            }
-        else{
-            myled2=0;
+
+            avancer(voiture_deltat);
+
+
+            // converversion vitesse/angle
+            int t_angle = convers_angle_tempsh(voiture_angle_roues);
+
+            if (voiture_angle_roues==0) {
+                myled2=1;
+            } else {
+                myled2=0;
             }
-            
-        int t_vitesse = convers_vitesse_tempsh(voiture_vitesse);
-        
-        if (1440 <=t_vitesse <= 1480){
-            myled3=0;
-            }
-        else{
-            myled3=1;
+
+            int t_vitesse = convers_vitesse_tempsh(voiture_vitesse);
+
+            if (1440 <=t_vitesse <= 1480) {
+                myled3=0;
+            } else {
+                myled3=1;
             }
-        
-        // commande des servos
-        pwm_servo_direction.pulsewidth_us(t_angle);
-        pwm_servo_vitesse.pulsewidth_us(t_vitesse);
-        
-       }
-   
+
+            // commande des servos
+            pwm_servo_direction.pulsewidth_us(t_angle);
+            pwm_servo_vitesse.pulsewidth_us(t_vitesse);
+
+        }
+    }
+
 }