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/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;
}

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

@ -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)