From ee8a1b9616c19c10ba18644f2fd3a325bfb653e0 Mon Sep 17 00:00:00 2001
From: Ulysse Cura <ulysse.cura@gmail.com>
Date: Wed, 7 May 2025 21:12:37 +0200
Subject: [PATCH] Corrected gyro, it's now functionnal !

---
 main controller code/src/gyro.c  | 26 ++++++++++++++++++++++----
 main controller code/src/robot.c |  6 ------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/main controller code/src/gyro.c b/main controller code/src/gyro.c
index 8ac80cb..dc18626 100644
--- a/main controller code/src/gyro.c	
+++ b/main controller code/src/gyro.c	
@@ -4,6 +4,8 @@
 #include "include/i2c_master.h"
 #include "include/robot.h"
 
+#include <stdio.h>
+
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Thank you Keuronde !                                                                              *
 *   https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c  *
@@ -33,6 +35,10 @@ int init_gyro(void)
 
     if(config_verification != config[1]) return -1;
 
+    robot.gyro_data.x_angle = 0.0f;
+    robot.gyro_data.y_angle = 0.0f;
+    robot.gyro_data.z_angle = 0.0f;
+
     return 0;
 }
 
@@ -50,7 +56,7 @@ static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t
 
 void gyro_calibrate(void)
 {
-    const uint nb_samples = 100;
+    const uint nb_samples = 1000;
 
     int16_t x, y, z;
     int32_t x_sum = 0, y_sum = 0, z_sum = 0;
@@ -58,7 +64,11 @@ void gyro_calibrate(void)
     for(uint i = 0; i < nb_samples; i++)
     {
         gyro_read(&x, &y, &z);
+
         x_sum += x;
+        y_sum += y;
+        z_sum += z;
+
         sleep_ms(SAMPLE_MIN_ELAPSED_TIME);
     }
 
@@ -68,6 +78,10 @@ void gyro_calibrate(void)
     //robot.gyro_data.x_offset = 0.0;
     //robot.gyro_data.y_offset = 0.0;
     //robot.gyro_data.z_offset = 0.0;
+
+    //printf("x_cal:%f\n", robot.gyro_data.x_offset);
+    //printf("y_cal:%f\n", robot.gyro_data.y_offset);
+    //printf("z_cal:%f\n", robot.gyro_data.z_offset);
 }
 
 static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps)
@@ -95,9 +109,13 @@ void gyro_update(void)
         y_dps -= robot.gyro_data.y_offset;
         z_dps -= robot.gyro_data.z_offset;
 
-        robot.gyro_data.x_angle += x_dps * elapsed_since_sample_ms * 1000.0f;
-        robot.gyro_data.y_angle += y_dps * elapsed_since_sample_ms * 1000.0f;
-        robot.gyro_data.z_angle += z_dps * elapsed_since_sample_ms * 1000.0f;
+        robot.gyro_data.x_angle += x_dps * elapsed_since_sample_ms / 1000.0f;
+        robot.gyro_data.y_angle += y_dps * elapsed_since_sample_ms / 1000.0f;
+        robot.gyro_data.z_angle += z_dps * elapsed_since_sample_ms / 1000.0f;
+
+        printf(">gyro_x_angle:%f\n", robot.gyro_data.x_angle);
+        printf(">gyro_y_angle:%f\n", robot.gyro_data.y_angle);
+        printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle);
 
         elapsed_since_sample_ms = 0.0;
     }
diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c
index a74824b..0224ef1 100644
--- a/main controller code/src/robot.c	
+++ b/main controller code/src/robot.c	
@@ -5,8 +5,6 @@
 #include "include/i2c_master.h"
 #include "include/gyro.h"
 
-#include <stdio.h>
-
 int robot_init(void)
 {
     stdio_init_all();
@@ -36,10 +34,6 @@ void robot_handle_inputs_outputs(void)
     update_time();
 
     gyro_update();
-
-    printf(">gyro_x:%f\n", robot.gyro_data.x_angle);
-    printf(">gyro_y:%f\n", robot.gyro_data.y_angle);
-    printf(">gyro_z:%f\n", robot.gyro_data.z_angle);
 }
 
 void robot_deinit(void)