Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Gyro globallement fonctionnel, mais la précision laisse à désirer

Browse Source
This commit is contained in:
Samuel 2022-09-26 21:53:52 +02:00
parent e98c500801
commit 317180158e
3 changed files with 92 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
gyro.c
View File

@ -5,24 +5,33 @@
#include "hardware/structs/spi.h" #include "hardware/structs/spi.h"
#include "spi_nb.h" #include "spi_nb.h"
#include "Temps.h" #include "Temps.h"
#include "gyro.h"
const uint PIN_CS = 1; const uint PIN_CS = 1;
/// @brief structure d'échange des angles du gyrocope
struct t_angle_gyro{ struct t_angle_gyro{
int32_t rot_x, rot_y, rot_z; 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(); int gyro_init_check();
void gyro_config(); void gyro_config();
int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire); 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); void gyro_calibration(void);
uint32_t rot_x_zero, rot_y_zero, rot_z_zero; 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){ void Gyro_Init(void){
@ -49,7 +58,7 @@ void Gyro_Init(void){
puts("Gyroscope trouve"); puts("Gyroscope trouve");
gyro_config(); gyro_config();
} }
//gyro_calibration(); gyro_calibration();
} }
int gyro_init_check(){ 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 SSPCPSR : %#4x\n", spi_get_hw(spi0)->cpsr );
//printf ("RPI SSPCR0 : %#4x\n", spi_get_hw(spi0)->cr0 ); //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 en degré
angle_y = angle_y + (double)angle_gyro.rot_y * step_ms * 0.001 * 0.00875 * 0.125; angle_gyro.rot_x = angle_gyro.rot_x + (double)vitesse_angulaire.rot_x * 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_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)); //while(spi_nb_busy(spi0));
//spi_nb_read_data_8bits(spi0,tampon); //spi_nb_read_data_8bits(spi0,tampon);
//printf("tampon : %s\n", 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"; uint8_t tampon[10]="\0\0\0\0\0\0\0\0\0";
int16_t rot_x, rot_y, rot_z; int16_t rot_x, rot_y, rot_z;
spi_read_register(spi0, 0x28, tampon, 6); 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_y = -(tampon[3] + (tampon[4] << 8));
rot_z = -(tampon[5] + (tampon[6] << 8)); rot_z = -(tampon[5] + (tampon[6] << 8));
angle_gyro->rot_x = (int32_t) rot_x * 8; if(angle_gyro_moy == NULL){
angle_gyro->rot_y = (int32_t) rot_y * 8; angle_gyro->rot_x = (int32_t) rot_x * 8;
angle_gyro->rot_z = (int32_t) rot_z * 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){ void gyro_calibration(void){
uint16_t nb_ech = 3000; uint32_t nb_ech = 3000;
uint32_t m_temps_ms = Temps_get_temps_ms(); uint32_t m_temps_ms = Temps_get_temps_ms();
printf("Calibration...\n"); printf("Calibration...\n");
angle_gyro_moy.rot_x = 0; vitesse_calibration.rot_x = 0;
angle_gyro_moy.rot_y = 0; vitesse_calibration.rot_y = 0;
angle_gyro_moy.rot_z = 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()); while(m_temps_ms == Temps_get_temps_ms());
gyro_get_angles(&angle_gyro); m_temps_ms = Temps_get_temps_ms();
angle_gyro_moy.rot_x += angle_gyro.rot_x; gyro_get_angles(&vitesse_angulaire, NULL);
angle_gyro_moy.rot_y += angle_gyro.rot_y; vitesse_calibration.rot_x += vitesse_angulaire.rot_x;
angle_gyro_moy.rot_z += angle_gyro.rot_z; vitesse_calibration.rot_y += vitesse_angulaire.rot_y;
} vitesse_calibration.rot_z += vitesse_angulaire.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;
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");
} }

6
gyro.h
View File

@ -1,2 +1,8 @@
struct t_angle_gyro_double{
double rot_x, rot_y, rot_z;
};
void Gyro_Init(void); void Gyro_Init(void);
void Gyro_Read(u_int16_t); 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);

35
test.c
View File

@ -4,6 +4,7 @@
#include "pico/binary_info.h" #include "pico/binary_info.h"
#include "gyro.h" #include "gyro.h"
#include "Temps.h"
const uint LED_PIN = 25; const uint LED_PIN = 25;
@ -11,6 +12,8 @@ int main() {
bi_decl(bi_program_description("This is a test binary.")); bi_decl(bi_program_description("This is a test binary."));
bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED")); bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
uint32_t temps_ms = 0;
stdio_init_all(); stdio_init_all();
gpio_init(LED_PIN); gpio_init(LED_PIN);
@ -19,16 +22,32 @@ int main() {
sleep_ms(3000); sleep_ms(3000);
Gyro_Init();
Temps_init();
Gyro_Init();
temps_ms = Temps_get_temps_ms();
while (1) { while (1) {
u_int16_t step_ms = 100; u_int16_t step_ms = 100;
/*gpio_put(LED_PIN, 0);
sleep_ms(251); // Tous les pas de step_ms
gpio_put(LED_PIN, 1); if(temps_ms == Temps_get_temps_ms()){
puts("Bonjour"); temps_ms += step_ms;
sleep_ms(1000);*/
sleep_ms(step_ms);
Gyro_Read(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 :");
}
} }
} }