Diff: main.cpp

Revision:

1:b1f5ab5f08a2

Parent:

0:0232ed5c62b6

diff -r 0232ed5c62b6 -r b1f5ab5f08a2 main.cpp
--- a/main.cpp	Tue Sep 24 11:56:20 2019 +0000
+++ b/main.cpp	Sat Sep 28 10:06:37 2019 +0000
@@ -22,6 +22,34 @@
 // declaration des IO
 
 DigitalOut led(LED1);
+DigitalOut enable_esc(PG_0);
+
+InterruptIn odoInput(PF_8);
+
+#define ROUE_DISTANCE_TIC_2019 0.06
+#define ROUE_DISTANCE_TIC_2018 0.08
+
+float roueDistanceTic = ROUE_DISTANCE_TIC_2019; //A changer en ROUE_DISTANCE_TIC_2018 pour chassis 2018
+float vitesseBiniou = 0.0; //En m/s
+float compteurDistance = 0.0;
+
+Timer odoTick;// temps.start()/stop()/sec: read()/ms: read_ms()/µs: read_us()
+
+void odoTickHandler()
+{
+    unsigned odoMicroSec = 0;
+    //Incremente compteur de distance
+    compteurDistance++;
+    //Calcule de la vitesse Biniou avec Odometre en fonction de horodatage du TIC odo precedent
+    vitesseBiniou = roueDistanceTic / (odoTick.read_ms()*1000);
+    
+    odoTick.reset();
+    odoTick.start();
+    
+    printf("ODO TICK!\n");
+    printf("vitesseBiniou: %f\n", vitesseBiniou);
+    printf("compteurDistance: %u\n", compteurDistance);
+}
 
 // declaration d'une interruption sur le front de la pin d'odometre
 //InterruptRise my_interruptPin(PF_8);
@@ -33,7 +61,7 @@
 //}
 
 // initialisation du MPU6050
-DigitalOut myled(LED1);
+
 Serial pc(USBTX, USBRX,115200);
 
 MPU6050 mpu;
@@ -94,6 +122,8 @@
         pc.printf("MPU6050 test failed \n");
     }
 
+    // Assign functions to Odometre input
+    odoInput.fall(&odoTickHandler);
 
     while(1) {
         // recuperer la valeur en float entre 0.0 et 1.0
@@ -126,25 +156,25 @@
 
         // affiche les valeurs sur la com usb
         printf("\f");
-     //   printf("meas_adc_PA4_V = %.0f mV\n\r", meas_adc_PA4_V);
-     //   printf("meas_adc_PA5_V = %.0f mV\n\r", meas_adc_PA5_V);
-    //    printf("meas_adc_PA6_V = %.0f mV\n\r", meas_adc_PA6_V);
-     //   printf("meas_adc_PC0_V = %.0f mV\n\r", meas_adc_PC0_V);
+        printf("meas_adc_PA4_V = %.0f mV\n\r", meas_adc_PA4_V);
+        printf("meas_adc_PA5_V = %.0f mV\n\r", meas_adc_PA5_V);
+        printf("meas_adc_PA6_V = %.0f mV\n\r", meas_adc_PA6_V);
+        printf("meas_adc_PC0_V = %.0f mV\n\r", meas_adc_PC0_V);
 
-     //   printf("meas_adc_PF3_V = %.0f mV\n\r", meas_adc_PF3_V);
-    //    printf("meas_adc_PF4_V = %.0f mV\n\r", meas_adc_PF4_V);
-     //   printf("meas_adc_PF9_V = %.0f mV\n\r", meas_adc_PF9_V);
-       // printf("meas_adc_PF10_V = %.0f mV\n\r", meas_adc_PF10_V);
+        printf("meas_adc_PF3_V = %.0f mV\n\r", meas_adc_PF3_V);
+        printf("meas_adc_PF4_V = %.0f mV\n\r", meas_adc_PF4_V);
+        printf("meas_adc_PF9_V = %.0f mV\n\r", meas_adc_PF9_V);
+        printf("meas_adc_PF10_V = %.0f mV\n\r", meas_adc_PF10_V);
 
 
         //writing current accelerometer and gyro position
 
         printf("acc X =%.2f; acc Y =%.2f; acc Z=%.2f;\n\r",ax_g,ay_g,az_g);
-        printf("Gyr X =%d; Gyr Y =%d; Gyr Z =%d\n",gx,gy,gz);
+
         //pc.printf("acc X =%d; acc Y =%d; acc Z=%d; Gyr X =%d; Gyr Y =%d; Gyr Z =%d\n",ax,ay,az,gx,gy,gz);
 
-
         led = !led;
-        wait(1.0); // delais
+        enable_esc = !enable_esc ;
+        wait(0.2); // delais
     }
 }