55 lines
1.3 KiB
C
55 lines
1.3 KiB
C
|
#include "Localisation.h"
|
||
|
|
#include "Temps.h"
|
||
|
|
#include "QEI.h"
|
||
|
|
#include "math.h"
|
||
|
|
#include "Geometrie_robot.h"
|
||
|
|
|
||
|
|
struct position_t position;
|
||
|
|
|
||
|
|
void Localisation_init(){
|
||
|
|
Temps_init();
|
||
|
|
QEI_init();
|
||
|
|
position.x_mm = 0;
|
||
|
|
position.y_mm = 0;
|
||
|
|
position.angle_radian = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Localisation_set_x(float x_mm){
|
||
|
|
position.x_mm = x_mm;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Localisation_set_y(float y_mm){
|
||
|
|
position.y_mm = y_mm;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Localisation_set_angle(float angle_radian){
|
||
|
|
position.angle_radian = angle_radian;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Localisation_gestion(){
|
||
|
|
float distance_roue_gauche_mm = QEI_get_mm(QEI_B_NAME);
|
||
|
|
float distance_roue_droite_mm = QEI_get_mm(QEI_A_NAME);
|
||
|
|
|
||
|
|
float delta_avance_mm, delta_orientation_rad;
|
||
|
|
|
||
|
|
delta_avance_mm = (distance_roue_droite_mm + distance_roue_gauche_mm)/2;
|
||
|
|
delta_orientation_rad = (distance_roue_droite_mm - distance_roue_gauche_mm) / (DISTANCE_ROUES_CENTRE_MM*2);
|
||
|
|
|
||
|
|
position.angle_radian += delta_orientation_rad;
|
||
|
|
|
||
|
|
// Projection dans le référentiel de la table
|
||
|
|
position.x_mm += delta_avance_mm * cosf(position.angle_radian);
|
||
|
|
position.y_mm += delta_avance_mm * sinf(position.angle_radian);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
struct position_t Localisation_get(void){
|
||
|
|
return position;
|
||
|
|
}
|