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

83
gyro.c
View File

@ -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));
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;
printf("Calibration : rx : %f, ry : %f, rz: %f\n", angle_gyro_moy.rot_x,
angle_gyro_moy.rot_y, angle_gyro_moy.rot_z);
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;
}
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_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);

23
test.c
View File

@ -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;
// 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);
//sleep_ms(step_ms);
Gyro_Read(step_ms);
}
if((Temps_get_temps_ms() % 500) == 0){
gyro_affiche(gyro_get_angle(), "Angle :");
}
}
}