ulysse
/
Robot_Principal_2024-2025
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Corrected gyro, it's now functionnal !

This commit is contained in:
Ulysse Cura 2025-05-07 21:12:37 +02:00
parent ece650ee9f
commit ee8a1b9616
2 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
main controller code/src/gyro.c
View File

@ -4,6 +4,8 @@
#include "include/i2c_master.h" #include "include/i2c_master.h"
#include "include/robot.h" #include "include/robot.h"
#include <stdio.h>
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
* Thank you Keuronde ! * * Thank you Keuronde ! *
* https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c * * 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; 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; return 0;
} }
@ -50,7 +56,7 @@ static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t
void gyro_calibrate(void) void gyro_calibrate(void)
{ {
const uint nb_samples = 100; const uint nb_samples = 1000;
int16_t x, y, z; int16_t x, y, z;
int32_t x_sum = 0, y_sum = 0, z_sum = 0; 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++) for(uint i = 0; i < nb_samples; i++)
{ {
gyro_read(&x, &y, &z); gyro_read(&x, &y, &z);
x_sum += x; x_sum += x;
y_sum += y;
z_sum += z;
sleep_ms(SAMPLE_MIN_ELAPSED_TIME); sleep_ms(SAMPLE_MIN_ELAPSED_TIME);
} }
@ -68,6 +78,10 @@ void gyro_calibrate(void)
//robot.gyro_data.x_offset = 0.0; //robot.gyro_data.x_offset = 0.0;
//robot.gyro_data.y_offset = 0.0; //robot.gyro_data.y_offset = 0.0;
//robot.gyro_data.z_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) 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; y_dps -= robot.gyro_data.y_offset;
z_dps -= robot.gyro_data.z_offset; z_dps -= robot.gyro_data.z_offset;
robot.gyro_data.x_angle += x_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.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.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; elapsed_since_sample_ms = 0.0;
} }

6
main controller code/src/robot.c
View File

@ -5,8 +5,6 @@
#include "include/i2c_master.h" #include "include/i2c_master.h"
#include "include/gyro.h" #include "include/gyro.h"
#include <stdio.h>
int robot_init(void) int robot_init(void)
{ {
stdio_init_all(); stdio_init_all();
@ -36,10 +34,6 @@ void robot_handle_inputs_outputs(void)
update_time(); update_time();
gyro_update(); 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) void robot_deinit(void)