89 lines
2.8 KiB
C
89 lines
2.8 KiB
C
#include "Strategie.h"
|
|
#include "Rotation.h"
|
|
#include "Localisation.h"
|
|
#include "Asser_Moteurs.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "math.h"
|
|
|
|
float rotation_angle_cible;
|
|
float rotation_angle_sens;
|
|
float rotation_vitesse_rad_s;
|
|
float rotation_vitesse_max_rad_s;
|
|
float rotation_acceleration_rad_ss;
|
|
|
|
float Rotation_calcul_vitesse(float angle_actuel, float pas_de_temps_s);
|
|
|
|
void rotation_init(float angle_cible){
|
|
struct position_t position = Localisation_get();
|
|
rotation_angle_cible = Geometrie_get_angle_optimal(position.angle_radian, angle_cible);
|
|
rotation_vitesse_rad_s = 0;
|
|
rotation_vitesse_max_rad_s = 5;
|
|
rotation_acceleration_rad_ss = 25;
|
|
|
|
if(rotation_angle_cible - position.angle_radian > 0){
|
|
rotation_angle_sens = 1;
|
|
}else{
|
|
rotation_angle_sens = -1;
|
|
}
|
|
|
|
}
|
|
|
|
void rotation_set_vitesse(float vitesse_rad_s){
|
|
float vitesse_mm_s = vitesse_rad_s * DISTANCE_ROUES_CENTRE_MM;
|
|
if(rotation_angle_sens == 1){
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_mm_s);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_mm_s);
|
|
}else{
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_mm_s);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_mm_s);
|
|
}
|
|
}
|
|
enum etat_action_t rotation_gestion(float pas_de_temps_s){
|
|
struct position_t position = Localisation_get();
|
|
rotation_vitesse_rad_s = Rotation_calcul_vitesse(position.angle_radian, pas_de_temps_s);
|
|
|
|
if (rotation_angle_sens == 1 ){
|
|
rotation_set_vitesse(rotation_vitesse_rad_s);
|
|
if (position.angle_radian > rotation_angle_cible){
|
|
return ACTION_TERMINEE;
|
|
}
|
|
|
|
}else{
|
|
if (position.angle_radian < rotation_angle_cible){
|
|
return ACTION_TERMINEE;
|
|
}
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
}
|
|
|
|
float Rotation_calcul_vitesse(float angle_actuel, float pas_de_temps_s){
|
|
// Phase d'accélération
|
|
rotation_vitesse_rad_s = rotation_vitesse_rad_s + pas_de_temps_s * rotation_acceleration_rad_ss;
|
|
|
|
// Saturation par vitesse max
|
|
if(rotation_vitesse_rad_s > rotation_vitesse_max_rad_s){
|
|
rotation_vitesse_rad_s = rotation_vitesse_max_rad_s;
|
|
}
|
|
|
|
// Saturation par la courbe de décélération
|
|
float vitesse_décélération_rad_s;
|
|
// distance restante
|
|
float angle_restant;
|
|
if (rotation_angle_sens == 1){
|
|
angle_restant = rotation_angle_cible - angle_actuel;
|
|
}else{
|
|
angle_restant = angle_actuel - rotation_angle_cible;
|
|
}
|
|
|
|
if(angle_restant < 0){
|
|
vitesse_décélération_rad_s = 0;
|
|
}else{
|
|
vitesse_décélération_rad_s = sqrtf(2 * rotation_acceleration_rad_ss * angle_restant);
|
|
}
|
|
if(rotation_vitesse_rad_s > vitesse_décélération_rad_s){
|
|
rotation_vitesse_rad_s = vitesse_décélération_rad_s;
|
|
}
|
|
return rotation_vitesse_rad_s;
|
|
}
|