RPiPico-Holonome2023/Localisation.c

64 lines
2.0 KiB
C
Raw Normal View History

#include "gyro.h"
#include "Localisation.h"
#include "QEI.h"
#include "math.h"
#include "Geometrie_robot.h"
#include "Robot_config.h"
struct position_t position;
void Localisation_init(){
position.x_mm = 0;
position.y_mm = 0;
position.angle_radian = 0;
}
2023-04-28 21:51:43 +00:00
void Localisation_set(float x_mm, float y_mm, float angle_radian){
position.x_mm = x_mm;
position.y_mm = y_mm;
position.angle_radian = angle_radian;
gyro_set_angle_radian(angle_radian);
}
2023-04-28 21:51:43 +00:00
void Localisation_set_x(float x_mm){
position.x_mm = x_mm;
}
2023-04-28 21:51:43 +00:00
void Localisation_set_y(float y_mm){
position.y_mm = y_mm;
}
2023-04-28 21:51:43 +00:00
void Localisation_set_angle(float angle_radian){
position.angle_radian = angle_radian;
}
void Localisation_gestion(){
2023-04-28 21:51:43 +00:00
struct t_angle_gyro_float angle_gyro;
// Voir http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html
2023-04-28 21:51:43 +00:00
float distance_roue_a_mm = QEI_get_mm(QEI_A_NAME);
float distance_roue_b_mm = QEI_get_mm(QEI_B_NAME);
float distance_roue_c_mm = QEI_get_mm(QEI_C_NAME);
float delta_x_ref_robot, delta_y_ref_robot;
2023-04-28 21:51:43 +00:00
float old_orientation_radian = position.angle_radian;
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 ;
}else{
position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM);
}
// Projection dans le référentiel de la table
2023-04-28 21:51:43 +00:00
position.x_mm += delta_x_ref_robot * cosf(position.angle_radian) - delta_y_ref_robot * sinf(position.angle_radian);
position.y_mm += delta_x_ref_robot * sinf(position.angle_radian) + delta_y_ref_robot * cosf(position.angle_radian);
}
struct position_t Localisation_get(void){
return position;
}