182 lines
7.5 KiB
C
182 lines
7.5 KiB
C
#include "Trajectoire.h"
|
|
#include "Trajectoire_bezier.h"
|
|
#include "Trajectoire_circulaire.h"
|
|
#include "Trajectoire_droite.h"
|
|
#include "Trajectoire_rotation.h"
|
|
|
|
#include "math.h"
|
|
|
|
#define NB_MAX_TRAJECTOIRES 5
|
|
#define PRECISION_ABSCISSE 0.001
|
|
|
|
|
|
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon,
|
|
double orientation_debut_rad, double orientation_fin_rad){
|
|
trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
|
|
trajectoire->p1.x = centre_x;
|
|
trajectoire->p1.y = centre_y;
|
|
trajectoire->angle_debut_degre = angle_debut_degre;
|
|
trajectoire->angle_fin_degre = angle_fin_degre;
|
|
trajectoire->rayon = rayon;
|
|
trajectoire->longueur = -1;
|
|
trajectoire->orientation_debut_rad = orientation_debut_rad;
|
|
trajectoire->orientation_fin_rad = orientation_fin_rad;
|
|
}
|
|
|
|
void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y,
|
|
double orientation_debut_rad, double orientation_fin_rad){
|
|
trajectoire->type = TRAJECTOIRE_DROITE;
|
|
trajectoire->p1.x = p1_x;
|
|
trajectoire->p1.y = p1_y;
|
|
trajectoire->p2.x = p2_x;
|
|
trajectoire->p2.y = p2_y;
|
|
trajectoire->longueur = -1;
|
|
trajectoire->orientation_debut_rad = orientation_debut_rad;
|
|
trajectoire->orientation_fin_rad = orientation_fin_rad;
|
|
}
|
|
|
|
void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y,
|
|
double orientation_debut_rad, double orientation_fin_rad){
|
|
trajectoire->type = TRAJECTOIRE_BEZIER;
|
|
trajectoire->p1.x = p1_x;
|
|
trajectoire->p1.y = p1_y;
|
|
trajectoire->p2.x = p2_x;
|
|
trajectoire->p2.y = p2_y;
|
|
trajectoire->p3.x = p3_x;
|
|
trajectoire->p3.y = p3_y;
|
|
trajectoire->p4.x = p4_x;
|
|
trajectoire->p4.y = p4_y;
|
|
trajectoire->longueur = -1;
|
|
trajectoire->orientation_debut_rad = orientation_debut_rad;
|
|
trajectoire->orientation_fin_rad = orientation_fin_rad;
|
|
}
|
|
|
|
void Trajectoire_rotation(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double orientation_debut_rad, double orientation_fin_rad){
|
|
trajectoire->type = TRAJECTOIRE_ROTATION;
|
|
trajectoire->p1.x = p1_x;
|
|
trajectoire->p1.y = p1_y;
|
|
trajectoire->longueur = -1;
|
|
trajectoire->orientation_debut_rad = orientation_debut_rad;
|
|
trajectoire->orientation_fin_rad = orientation_fin_rad;
|
|
}
|
|
|
|
void Trajectoire_inverse(struct trajectoire_t * trajectoire){
|
|
struct trajectoire_t old_trajectoire;
|
|
old_trajectoire = *trajectoire;
|
|
|
|
trajectoire->orientation_debut_rad = old_trajectoire.orientation_fin_rad;
|
|
trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;
|
|
|
|
if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
|
|
trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
|
|
trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
|
|
return;
|
|
}
|
|
if(trajectoire->type == TRAJECTOIRE_DROITE){
|
|
trajectoire->p1 = old_trajectoire.p2;
|
|
trajectoire->p2 = old_trajectoire.p1;
|
|
return;
|
|
}
|
|
if(trajectoire->type == TRAJECTOIRE_BEZIER){
|
|
trajectoire->p1 = old_trajectoire.p4;
|
|
trajectoire->p2 = old_trajectoire.p3;
|
|
trajectoire->p3 = old_trajectoire.p2;
|
|
trajectoire->p4 = old_trajectoire.p1;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin
|
|
/// @param trajectoire
|
|
/// @return Longueur de la trajectoire
|
|
double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
|
|
if(trajectoire->longueur > 0){
|
|
// La longueur est déjà calculée
|
|
}else{
|
|
// Calculons la longueur de la trajectoire
|
|
switch(trajectoire->type){
|
|
case TRAJECTOIRE_DROITE:
|
|
Trajectoire_droite_get_longueur(trajectoire);
|
|
break;
|
|
case TRAJECTOIRE_CIRCULAIRE:
|
|
Trajectoire_circulaire_get_longueur(trajectoire);
|
|
break;
|
|
|
|
case TRAJECTOIRE_BEZIER:
|
|
Trajectoire_bezier_get_longueur(trajectoire);
|
|
break;
|
|
}
|
|
}
|
|
return trajectoire->longueur;
|
|
}
|
|
|
|
/// @brief Renvoie le point d'une trajectoire à partir de son abscisse
|
|
/// @param abscisse : abscisse sur la trajectoire
|
|
/// @return point en coordonnées X/Y
|
|
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
|
|
struct point_xyo_t point_xyo;
|
|
switch(trajectoire->type){
|
|
case TRAJECTOIRE_DROITE:
|
|
point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse);
|
|
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
|
|
break;
|
|
|
|
case TRAJECTOIRE_CIRCULAIRE:
|
|
point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse);
|
|
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
|
|
break;
|
|
|
|
case TRAJECTOIRE_BEZIER:
|
|
point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse);
|
|
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
|
|
break;
|
|
|
|
case TRAJECTOIRE_ROTATION:
|
|
point_xyo.point_xy = Trajectoire_rotation_get_point(trajectoire, abscisse);
|
|
point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
|
|
break;
|
|
}
|
|
return point_xyo;
|
|
}
|
|
|
|
double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse){
|
|
return (double) trajectoire->orientation_debut_rad * (1-abscisse) + (double) trajectoire->orientation_fin_rad * abscisse;
|
|
}
|
|
|
|
/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée
|
|
/// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire
|
|
/// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire
|
|
/// @return nouvelle abscisse
|
|
double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){
|
|
double delta_abscisse, delta_mm, erreur_relative;
|
|
|
|
if(distance_mm == 0){
|
|
return abscisse;
|
|
}
|
|
// Ceci permet d'avoir une abscisse exact sur les trajectoires droites, les trajectoires circulaires et les rotations
|
|
delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire);
|
|
if(trajectoire->type == TRAJECTOIRE_ROTATION || trajectoire->type == TRAJECTOIRE_CIRCULAIRE || trajectoire->type == TRAJECTOIRE_DROITE){
|
|
return abscisse + delta_abscisse;
|
|
}
|
|
|
|
delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
|
|
|
|
// Sur les trajectoires de bézier, il peut être nécessaire d'affiner
|
|
// Les cas où l'algorythme diverge ne devraient pas se produire car distance_cm << longeur_trajectoire.
|
|
erreur_relative = 1 - delta_mm / distance_mm;
|
|
while(fabs(erreur_relative) > PRECISION_ABSCISSE){
|
|
delta_abscisse = delta_abscisse * distance_mm / delta_mm;
|
|
delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
|
|
erreur_relative = 1 - delta_mm / distance_mm;
|
|
}
|
|
|
|
return abscisse + delta_abscisse;
|
|
}
|
|
|
|
double distance_points(struct point_xy_t point, struct point_xy_t point_old){
|
|
return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2));
|
|
|
|
}
|