diff --git a/Localisation.c b/Localisation.c index b7b6c32..dd530dc 100644 --- a/Localisation.c +++ b/Localisation.c @@ -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_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()){ - angle_gyro = gyro_get_angle_degres(); - position.angle_radian = angle_gyro.rot_z / 180. * M_PI ; + if(get_position_avec_gyroscope()){ + if(!get_position_avec_gyroscope_error()){ + 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{ position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM); } diff --git a/gyro.c b/gyro.c index b4ed321..a19d9d1 100644 --- a/gyro.c +++ b/gyro.c @@ -3,15 +3,19 @@ #include "hardware/gpio.h" #include "hardware/spi.h" #include "hardware/structs/spi.h" +#include "Robot_config.h" #include "Geometrie.h" #include "Monitoring.h" #include "spi_nb.h" +#include "Log.h" #include "Temps.h" #include "gyro.h" #include "math.h" #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 #include "gyro_L3GD20H.h" @@ -123,6 +127,11 @@ void Gyro_Read(uint16_t step_ms){ 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 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;