diff --git a/Trajectoire.c b/Trajectoire.c index 89ee335..5d56985 100644 --- a/Trajectoire.c +++ b/Trajectoire.c @@ -7,6 +7,7 @@ #include "math.h" #define PRECISION_ABSCISSE 0.001f +#define NB_MAX_ITERATIONS 3 void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, @@ -172,16 +173,18 @@ float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, do if(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){ + int nb_iteration=0; + while(fabs(erreur_relative) > PRECISION_ABSCISSE && nb_iteration < NB_MAX_ITERATIONS){ 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; + nb_iteration++; } return abscisse + delta_abscisse; diff --git a/config_robot.h b/config_robot.h index 9c7a436..7ecba7c 100644 --- a/config_robot.h +++ b/config_robot.h @@ -1,7 +1,7 @@ // Changer le define en fonction qu'on soit sur les PAMIs ou sur le robot principal -#define ROBOT_PROPULSION_2026 -//#define ROBOT_TYPE_PAMI +//#define ROBOT_PROPULSION_2026 +#define ROBOT_TYPE_PAMI #ifndef ROBOT_PROPULSION_2026 #ifndef ROBOT_TYPE_PAMI