2024-04-28 21:12:34 +00:00
|
|
|
|
#include <math.h>
|
|
|
|
|
#include "Geometrie.h"
|
|
|
|
|
#include "Trajectoire.h"
|
|
|
|
|
#include "Trajet.h"
|
|
|
|
|
#include "Asser_Position.h"
|
|
|
|
|
#include "Asser_Moteurs.h"
|
|
|
|
|
#include "Temps.h"
|
|
|
|
|
|
|
|
|
|
float Trajet_calcul_vitesse(float temps_s);
|
|
|
|
|
int Trajet_terminee(float abscisse);
|
|
|
|
|
|
|
|
|
|
float abscisse; // Position entre 0 et 1 sur la trajectoire
|
|
|
|
|
float position_mm; // Position en mm sur la trajectoire
|
|
|
|
|
float vitesse_mm_s;
|
|
|
|
|
float vitesse_max_trajet_mm_s=500;
|
|
|
|
|
float acceleration_mm_ss;
|
|
|
|
|
const float acceleration_mm_ss_obstacle = 500;
|
|
|
|
|
struct trajectoire_t trajet_trajectoire;
|
|
|
|
|
struct position_t position_consigne;
|
|
|
|
|
|
|
|
|
|
float distance_obstacle_mm;
|
|
|
|
|
float distance_fin_trajectoire_mm;
|
|
|
|
|
const float distance_pas_obstacle = 2000;
|
|
|
|
|
|
|
|
|
|
float vitesse_max_contrainte_obstacle;
|
|
|
|
|
|
|
|
|
|
/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
|
2024-05-09 14:58:54 +00:00
|
|
|
|
void Trajet_init(int id){
|
2024-04-28 21:12:34 +00:00
|
|
|
|
Temps_init();
|
2024-05-09 14:58:54 +00:00
|
|
|
|
AsserMoteur_Init(id);
|
2024-04-28 21:12:34 +00:00
|
|
|
|
abscisse = 0;
|
|
|
|
|
vitesse_mm_s = 0;
|
|
|
|
|
position_mm = 0;
|
|
|
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets
|
|
|
|
|
/// @param _vitesse_max_trajet_mm_s
|
|
|
|
|
/// @param _acceleration_mm_ss
|
|
|
|
|
void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss){
|
|
|
|
|
vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s;
|
|
|
|
|
acceleration_mm_ss = _acceleration_mm_ss;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
|
|
|
|
|
abscisse = 0;
|
|
|
|
|
vitesse_mm_s = 0;
|
|
|
|
|
position_mm = 0;
|
|
|
|
|
trajet_trajectoire = trajectoire;
|
|
|
|
|
Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// @brief Avance la consigne de position sur la trajectoire
|
|
|
|
|
/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde
|
|
|
|
|
/// @return TRAJET_EN_COURS ou TRAJET_TERMINE
|
2024-04-30 15:59:41 +00:00
|
|
|
|
struct point_xyo_t point;
|
2024-04-28 21:12:34 +00:00
|
|
|
|
enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
|
|
|
|
|
float distance_mm;
|
|
|
|
|
enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;
|
2024-04-30 15:59:41 +00:00
|
|
|
|
|
2024-04-28 21:12:34 +00:00
|
|
|
|
struct position_t position;
|
|
|
|
|
|
|
|
|
|
// Calcul de la vitesse
|
|
|
|
|
vitesse_mm_s = Trajet_calcul_vitesse(pas_de_temps_s);
|
|
|
|
|
|
|
|
|
|
// Calcul de l'avancement en mm
|
|
|
|
|
distance_mm = vitesse_mm_s * pas_de_temps_s;
|
|
|
|
|
position_mm += distance_mm;
|
|
|
|
|
|
|
|
|
|
// Calcul de l'abscisse sur la trajectoire
|
|
|
|
|
abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm);
|
|
|
|
|
//set_debug_varf(abscisse);
|
|
|
|
|
|
|
|
|
|
// Obtention du point consigne
|
|
|
|
|
point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
|
|
|
|
|
|
|
|
|
|
position.x_mm = point.point_xy.x;
|
|
|
|
|
position.y_mm = point.point_xy.y;
|
|
|
|
|
position.angle_radian = point.orientation;
|
|
|
|
|
|
|
|
|
|
position_consigne=position;
|
|
|
|
|
Asser_Position(position);
|
|
|
|
|
|
|
|
|
|
if(Trajet_terminee(abscisse)){
|
|
|
|
|
Asser_Position_set_Pos_Maintien(position);
|
|
|
|
|
trajet_etat = TRAJET_TERMINE;
|
|
|
|
|
}
|
|
|
|
|
return trajet_etat;
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Trajet_stop(float pas_de_temps_s){
|
|
|
|
|
vitesse_mm_s = 0;
|
|
|
|
|
Trajet_avance(0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier
|
|
|
|
|
/// où les approximations font que l'abscisse peut ne pas atteindre 1.
|
|
|
|
|
/// @param abscisse : abscisse sur la trajectoire
|
|
|
|
|
/// @return 1 si le trajet est terminé, 0 sinon
|
|
|
|
|
int Trajet_terminee(float abscisse){
|
|
|
|
|
/*if(abscisse >= 0.99 ){
|
|
|
|
|
return 1;
|
|
|
|
|
}*/
|
|
|
|
|
|
|
|
|
|
if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
|
|
|
|
|
if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){
|
|
|
|
|
return 1;
|
|
|
|
|
}
|
|
|
|
|
}else{
|
|
|
|
|
if(abscisse >= 0.99 ){
|
|
|
|
|
return 1;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// @brief Envoie la consigne de position calculée par le module trajet. Principalement pour le débug/réglage asservissement.
|
|
|
|
|
struct position_t Trajet_get_consigne(){
|
|
|
|
|
return position_consigne;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire
|
|
|
|
|
/// @param pas_de_temps_s : temps écoulé en ms
|
|
|
|
|
/// @return vitesse déterminée en m/s
|
|
|
|
|
float Trajet_calcul_vitesse(float pas_de_temps_s){
|
|
|
|
|
float vitesse_max_contrainte;
|
|
|
|
|
float distance_contrainte,distance_contrainte_obstacle;
|
|
|
|
|
float vitesse;
|
|
|
|
|
// Calcul de la vitesse avec acceleration
|
|
|
|
|
vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s;
|
|
|
|
|
|
|
|
|
|
// Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s)
|
|
|
|
|
// https://poivron-robotique.fr/Consigne-de-vitesse.html
|
|
|
|
|
distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
|
|
|
|
|
distance_fin_trajectoire_mm=distance_contrainte;
|
|
|
|
|
// En cas de dépassement, on veut garder la contrainte, pour l'instant
|
|
|
|
|
if(distance_contrainte > 0){
|
|
|
|
|
vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte);
|
|
|
|
|
}else{
|
|
|
|
|
vitesse_max_contrainte = 0;
|
|
|
|
|
}
|
|
|
|
|
|
2024-05-09 12:50:12 +00:00
|
|
|
|
distance_contrainte_obstacle = Trajet_get_obstacle_mm();
|
2024-04-28 21:12:34 +00:00
|
|
|
|
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
|
|
|
|
|
vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
|
|
|
|
|
if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
|
|
|
|
|
vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
|
|
|
|
|
}
|
2024-05-09 12:50:12 +00:00
|
|
|
|
}/*
|
2024-05-09 06:36:35 +00:00
|
|
|
|
if((Trajet_get_obstacle_mm() != DISTANCE_INVALIDE) && (Trajet_get_obstacle_mm() < 50)){
|
2024-05-07 07:56:57 +00:00
|
|
|
|
vitesse = 0;
|
2024-05-09 12:50:12 +00:00
|
|
|
|
}*/
|
2024-04-28 21:12:34 +00:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Selection de la vitesse la plus faible
|
|
|
|
|
if(vitesse > vitesse_max_contrainte){
|
|
|
|
|
vitesse = vitesse_max_contrainte;
|
|
|
|
|
}
|
|
|
|
|
if(vitesse > vitesse_max_trajet_mm_s){
|
|
|
|
|
vitesse = vitesse_max_trajet_mm_s;
|
|
|
|
|
}
|
|
|
|
|
return vitesse;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float Trajet_get_obstacle_mm(void){
|
|
|
|
|
return distance_obstacle_mm;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Trajet_set_obstacle_mm(float distance_mm){
|
|
|
|
|
distance_obstacle_mm = distance_mm;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain
|
|
|
|
|
/// @return angle en radian.
|
|
|
|
|
float Trajet_get_orientation_avance(){
|
|
|
|
|
struct point_xyo_t point, point_suivant;
|
|
|
|
|
float avance_abscisse = 0.01;
|
|
|
|
|
float angle;
|
|
|
|
|
|
|
|
|
|
if(abscisse >= 1){
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
if(abscisse + avance_abscisse >= 1){
|
|
|
|
|
avance_abscisse = 1 - abscisse;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
|
|
|
|
|
point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse);
|
|
|
|
|
|
|
|
|
|
angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
|
|
|
|
|
return angle;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Trajet_inverse(){
|
|
|
|
|
float old_abscisse = abscisse;
|
|
|
|
|
float old_position_mm = position_mm;
|
|
|
|
|
Trajectoire_inverse(&trajet_trajectoire);
|
|
|
|
|
Trajet_debut_trajectoire(trajet_trajectoire);
|
|
|
|
|
abscisse = 1 - old_abscisse;
|
|
|
|
|
position_mm = Trajectoire_get_longueur_mm(&trajet_trajectoire) - old_position_mm;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float Trajet_get_abscisse(){
|
|
|
|
|
return abscisse;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// @brief Indique si le robot est bloqué sur le trajet
|
|
|
|
|
/// @return 0 si le robot n'est pas bloqué, 1 s'il est bloqué
|
|
|
|
|
uint32_t Trajet_get_bloque(){
|
|
|
|
|
if(Trajet_get_obstacle_mm() == DISTANCE_INVALIDE){
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
if (vitesse_max_contrainte_obstacle == 0){
|
|
|
|
|
return 1;
|
|
|
|
|
}
|
|
|
|
|
return 0;
|
|
|
|
|
}
|