From 317180158e3781fcdcdf9b6ab6cacab4041d7c35 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Mon, 26 Sep 2022 21:53:52 +0200
Subject: [PATCH] =?UTF-8?q?Gyro=20globallement=20fonctionnel,=20mais=20la?=
 =?UTF-8?q?=20pr=C3=A9cision=20laisse=20=C3=A0=20d=C3=A9sirer?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 gyro.c | 87 +++++++++++++++++++++++++++++++++++++++-------------------
 gyro.h |  6 ++++
 test.c | 35 +++++++++++++++++------
 3 files changed, 92 insertions(+), 36 deletions(-)

diff --git a/gyro.c b/gyro.c
index 8577570..eba2a30 100644
--- a/gyro.c
+++ b/gyro.c
@@ -5,24 +5,33 @@
 #include "hardware/structs/spi.h"
 #include "spi_nb.h"
 #include "Temps.h"
+#include "gyro.h"
 
 const uint PIN_CS = 1;
 
-/// @brief structure d'échange des angles du gyrocope
 struct t_angle_gyro{
     int32_t rot_x, rot_y, rot_z;
-} angle_gyro, angle_gyro_moy;
+};
+
+/// @brief structure d'échange des angles du gyrocope
+struct t_angle_gyro vitesse_angulaire, vitesse_calibration;
 
 int gyro_init_check();
 void gyro_config();
 int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire);
-void gyro_get_angles(struct t_angle_gyro* angle_gyro);
+void gyro_get_angles(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
 void gyro_calibration(void);
 
 uint32_t rot_x_zero, rot_y_zero, rot_z_zero;
 
 
 
+struct t_angle_gyro_double angle_gyro;
+
+
+struct t_angle_gyro_double gyro_get_angle(void){
+    return angle_gyro;
+}
 
 
 void Gyro_Init(void){
@@ -49,7 +58,7 @@ void Gyro_Init(void){
         puts("Gyroscope trouve");
         gyro_config();
     }
-    //gyro_calibration();
+    gyro_calibration();
 }
 
 int gyro_init_check(){
@@ -129,20 +138,22 @@ void Gyro_Read(uint16_t step_ms){
     //printf ("RPI SSPCPSR : %#4x\n", spi_get_hw(spi0)->cpsr );
     //printf ("RPI SSPCR0 : %#4x\n", spi_get_hw(spi0)->cr0 );
 
-    gyro_get_angles(&angle_gyro);
+    //gyro_get_angles(&angle_gyro, &angle_gyro_calibration);
+    gyro_get_angles(&vitesse_angulaire, NULL);
 
-    angle_x = angle_x + (double)angle_gyro.rot_x * step_ms * 0.001 * 0.00875 * 0.125;
-    angle_y = angle_y + (double)angle_gyro.rot_y * step_ms * 0.001 * 0.00875 * 0.125;
-    angle_z = angle_z + (double)angle_gyro.rot_z * step_ms * 0.001 * 0.00875 * 0.125;
+    // Angle en degré
+    angle_gyro.rot_x = angle_gyro.rot_x + (double)vitesse_angulaire.rot_x * step_ms * 0.001 * 0.00875 * 0.125;
+    angle_gyro.rot_y = angle_gyro.rot_y + (double)vitesse_angulaire.rot_y * step_ms * 0.001 * 0.00875 * 0.125;
+    angle_gyro.rot_z = angle_gyro.rot_z + (double)vitesse_angulaire.rot_z * step_ms * 0.001 * 0.00875 * 0.125;
     
-    printf("rx : %f, ry : %f, rz: %f\n", angle_x, angle_y, angle_z);
+    //printf("rx : %f, ry : %f, rz: %f\n", angle_gyro.rot_x, angle_gyro.rot_y, angle_gyro.rot_z);
 
     //while(spi_nb_busy(spi0));
     //spi_nb_read_data_8bits(spi0,tampon);
     //printf("tampon : %s\n", tampon);
 }
 
-void gyro_get_angles(struct t_angle_gyro* angle_gyro){
+void gyro_get_angles(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy){
     uint8_t tampon[10]="\0\0\0\0\0\0\0\0\0";
     int16_t rot_x, rot_y, rot_z;
     spi_read_register(spi0, 0x28, tampon, 6);
@@ -151,36 +162,56 @@ void gyro_get_angles(struct t_angle_gyro* angle_gyro){
     rot_y = -(tampon[3] + (tampon[4] << 8));
     rot_z = -(tampon[5] + (tampon[6] << 8));
 
-    angle_gyro->rot_x = (int32_t) rot_x * 8;
-    angle_gyro->rot_y = (int32_t) rot_y * 8;
-    angle_gyro->rot_z = (int32_t) rot_z * 8;
+    if(angle_gyro_moy == NULL){
+        angle_gyro->rot_x = (int32_t) rot_x * 8;
+        angle_gyro->rot_y = (int32_t) rot_y * 8;
+        angle_gyro->rot_z = (int32_t) rot_z * 8;
+    }else{
+        angle_gyro->rot_x = (int32_t) rot_x * 8 - angle_gyro_moy->rot_x;
+        angle_gyro->rot_y = (int32_t) rot_y * 8 - angle_gyro_moy->rot_y;
+        angle_gyro->rot_z = (int32_t) rot_z * 8 - angle_gyro_moy->rot_z;
+    }
+
+    
 
     
 
+}
+
+void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){
+    if(titre != NULL){
+        printf("%s ",titre);
+    }
+    printf("rx : %f, ry : %f, rz: %f\n", angle_gyro.rot_x, 
+    angle_gyro.rot_y, angle_gyro.rot_z);
 }
 
 void gyro_calibration(void){
-    uint16_t nb_ech = 3000;
+    uint32_t nb_ech = 3000;
     uint32_t m_temps_ms = Temps_get_temps_ms();
 
     printf("Calibration...\n");
 
-    angle_gyro_moy.rot_x = 0;
-    angle_gyro_moy.rot_y = 0;
-    angle_gyro_moy.rot_z = 0;
+    vitesse_calibration.rot_x = 0;
+    vitesse_calibration.rot_y = 0;
+    vitesse_calibration.rot_z = 0;
     
-    for(uint16_t i=0; i<nb_ech; i++){
+    // Acquisition des échantillons, 1 par milliseconde
+    for(uint32_t i=0; i<nb_ech; i++){
         while(m_temps_ms == Temps_get_temps_ms());
-        gyro_get_angles(&angle_gyro);
-        angle_gyro_moy.rot_x += angle_gyro.rot_x;
-        angle_gyro_moy.rot_y += angle_gyro.rot_y;
-        angle_gyro_moy.rot_z += angle_gyro.rot_z;
-    }
-    angle_gyro_moy.rot_x = angle_gyro_moy.rot_x / nb_ech;
-    angle_gyro_moy.rot_y = angle_gyro_moy.rot_y / nb_ech;
-    angle_gyro_moy.rot_z = angle_gyro_moy.rot_z / nb_ech;
+        m_temps_ms = Temps_get_temps_ms();
+        gyro_get_angles(&vitesse_angulaire, NULL);
+        vitesse_calibration.rot_x += vitesse_angulaire.rot_x;
+        vitesse_calibration.rot_y += vitesse_angulaire.rot_y;
+        vitesse_calibration.rot_z += vitesse_angulaire.rot_z;
 
-    printf("Calibration : rx : %f, ry : %f, rz: %f\n", angle_gyro_moy.rot_x, 
-    angle_gyro_moy.rot_y, angle_gyro_moy.rot_z);
+    }
+    vitesse_calibration.rot_x = vitesse_calibration.rot_x / nb_ech;
+    vitesse_calibration.rot_y = vitesse_calibration.rot_y / nb_ech;
+    vitesse_calibration.rot_z = vitesse_calibration.rot_z / nb_ech;
+
+    printf("fin calibration\n");
 
 }
+
+
diff --git a/gyro.h b/gyro.h
index 27d421b..c4a7a21 100644
--- a/gyro.h
+++ b/gyro.h
@@ -1,2 +1,8 @@
+struct t_angle_gyro_double{
+    double rot_x, rot_y, rot_z;
+};
+
 void Gyro_Init(void);
 void Gyro_Read(u_int16_t);
+void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre);
+struct t_angle_gyro_double gyro_get_angle(void);
\ No newline at end of file
diff --git a/test.c b/test.c
index e876e8c..a799fbe 100644
--- a/test.c
+++ b/test.c
@@ -4,6 +4,7 @@
 #include "pico/binary_info.h"
 
 #include "gyro.h"
+#include "Temps.h"
 
 const uint LED_PIN = 25;
 
@@ -11,6 +12,8 @@ int main() {
     bi_decl(bi_program_description("This is a test binary."));
     bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
 
+    uint32_t temps_ms = 0;
+
     stdio_init_all();
 
     gpio_init(LED_PIN);
@@ -19,16 +22,32 @@ int main() {
 
     sleep_ms(3000);
 
-    Gyro_Init();
 
+    Temps_init();
+    Gyro_Init();
+    temps_ms = Temps_get_temps_ms();
     while (1) { 
         u_int16_t step_ms = 100;
-        /*gpio_put(LED_PIN, 0);
-        sleep_ms(251);
-        gpio_put(LED_PIN, 1);
-        puts("Bonjour");
-        sleep_ms(1000);*/
-        sleep_ms(step_ms);
-        Gyro_Read(step_ms);
+        
+        // Tous les pas de step_ms
+        if(temps_ms == Temps_get_temps_ms()){
+            temps_ms += step_ms;
+
+
+            
+            
+            /*gpio_put(LED_PIN, 0);
+            sleep_ms(251);
+            gpio_put(LED_PIN, 1);
+            puts("Bonjour");
+            sleep_ms(1000);*/
+            //sleep_ms(step_ms);
+            Gyro_Read(step_ms);
+            
+            
+        }
+        if((Temps_get_temps_ms() % 500) == 0){
+            gyro_affiche(gyro_get_angle(), "Angle :");
+        }
     }
 }