109 lines
3.5 KiB
C
109 lines
3.5 KiB
C
#include <math.h>
|
||
#include "Geometrie.h"
|
||
#include "Trajectoire.h"
|
||
#include "Trajet.h"
|
||
#include "Asser_Position.h"
|
||
|
||
double Trajet_calcul_vitesse(double temps_s);
|
||
|
||
double abscisse; // Position entre 0 et 1 sur la trajectoire
|
||
double position_mm; // Position en mm sur la trajectoire
|
||
double vitesse_mm_s;
|
||
double vitesse_max_trajet_mm_s=500;
|
||
double acceleration_mm_ss;
|
||
struct trajectoire_t trajet_trajectoire;
|
||
struct position_t position_consigne;
|
||
|
||
/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
|
||
void Trajet_init(){
|
||
abscisse = 0;
|
||
vitesse_mm_s = 0;
|
||
acceleration_mm_ss = 500;
|
||
position_mm = 0;
|
||
}
|
||
|
||
/// @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(double _vitesse_max_trajet_mm_s, double _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;
|
||
}
|
||
|
||
/// @brief Avance la consigne de position sur la trajectoire
|
||
/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde
|
||
enum etat_trajet_t Trajet_avance(double pas_de_temps_s){
|
||
double distance_mm;
|
||
enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;
|
||
struct point_xyo_t point;
|
||
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);
|
||
|
||
// 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(abscisse >= 1 ){
|
||
trajet_etat = TRAJET_TERMINE;
|
||
}
|
||
return trajet_etat;
|
||
|
||
}
|
||
|
||
/// @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
|
||
double Trajet_calcul_vitesse(double pas_de_temps_s){
|
||
double vitesse_max_contrainte;
|
||
double distance_contrainte;
|
||
double 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;
|
||
// En cas de dépassement, on veut garder la contrainte, pour l'instant
|
||
if(distance_contrainte > 0){
|
||
vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte);
|
||
}else{
|
||
vitesse_max_contrainte = 0;
|
||
}
|
||
|
||
// 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;
|
||
}
|