Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Garder une orientation cohérente lors des défaillances du Gyro + détecter les défaillances du gyro par une vitesse anomalement élevée

Browse Source
This commit is contained in:
Samuel 2023-08-19 20:39:59 +02:00
parent feed3bce25
commit 5d25fa8919
2 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Localisation.c
View File

@ -45,9 +45,19 @@ void Localisation_gestion(){
delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm) / 3.0; delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm) / 3.0;
delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm) * RACINE_DE_3 / 3.0; delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm) * RACINE_DE_3 / 3.0;
if(get_position_avec_gyroscope() && !get_position_avec_gyroscope_error()){ if(get_position_avec_gyroscope()){
angle_gyro = gyro_get_angle_degres(); if(!get_position_avec_gyroscope_error()){
position.angle_radian = angle_gyro.rot_z / 180. * M_PI ; angle_gyro = gyro_get_angle_degres();
position.angle_radian = angle_gyro.rot_z / 180. * M_PI ;
}else{
// On utilise les roues codeuses pour la position du robot.
float variation_angle = - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM);
position.angle_radian += variation_angle;
// On utilise aussi les roue codeuses pour ajuster la position du gyroscope tant qu'il est indisponible
gyro_set_angle_radian(position.angle_radian )
}
}else{ }else{
position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM); position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM);
} }

9
gyro.c
View File

@ -3,15 +3,19 @@
#include "hardware/gpio.h" #include "hardware/gpio.h"
#include "hardware/spi.h" #include "hardware/spi.h"
#include "hardware/structs/spi.h" #include "hardware/structs/spi.h"
#include "Robot_config.h"
#include "Geometrie.h" #include "Geometrie.h"
#include "Monitoring.h" #include "Monitoring.h"
#include "spi_nb.h" #include "spi_nb.h"
#include "Log.h"
#include "Temps.h" #include "Temps.h"
#include "gyro.h" #include "gyro.h"
#include "math.h" #include "math.h"
#define RADIAN_VERS_DEGRES (180. / M_PI) #define RADIAN_VERS_DEGRES (180. / M_PI)
// Vitesse maximale lue par le gyroscope en degrés par seconde
#define V_MAX_GYRO 400.
#ifdef GYRO_L3GD20H #ifdef GYRO_L3GD20H
#include "gyro_L3GD20H.h" #include "gyro_L3GD20H.h"
@ -123,6 +127,11 @@ void Gyro_Read(uint16_t step_ms){
vitesse_gyro = *vitesse_angulaire; vitesse_gyro = *vitesse_angulaire;
if(vitesse_angulaire->rot_x > V_MAX_GYRO || vitesse_angulaire->rot_x < -V_MAX_GYRO){
set_position_avec_gyroscope_error(1);
Log_message("GYRO > V_MAX_GYRO", ERROR);
}
// Intégration en fonction du pas de temps // Intégration en fonction du pas de temps
angle_gyro.rot_x = angle_gyro.rot_x + vitesse_angulaire->rot_x * step_ms * 0.001; angle_gyro.rot_x = angle_gyro.rot_x + vitesse_angulaire->rot_x * step_ms * 0.001;
angle_gyro.rot_y = angle_gyro.rot_y + vitesse_angulaire->rot_y * step_ms * 0.001; angle_gyro.rot_y = angle_gyro.rot_y + vitesse_angulaire->rot_y * step_ms * 0.001;