From 4d5e6c62531fc95ac92d58fbaff57d1b90ab0de9 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 28 Apr 2024 23:12:34 +0200 Subject: [PATCH] =?UTF-8?q?R=C3=A9glage=20de=20l'asservissement=20avec=20p?= =?UTF-8?q?ile=209V?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Asser_Position.c | 4 +- CMakeLists.txt | 5 + Trajectoire.c | 167 ++++++++++++++++++++++++++++++ Trajectoire.h | 40 ++++++++ Trajectoire_bezier.c | 35 +++++++ Trajectoire_bezier.h | 5 + Trajectoire_circulaire.c | 26 +++++ Trajectoire_circulaire.h | 4 + Trajectoire_droite.c | 17 +++ Trajectoire_droite.h | 4 + Trajet.c | 216 +++++++++++++++++++++++++++++++++++++++ Trajet.h | 36 +++++++ main.c | 35 +++++-- 13 files changed, 581 insertions(+), 13 deletions(-) create mode 100644 Trajectoire.c create mode 100644 Trajectoire.h create mode 100644 Trajectoire_bezier.c create mode 100644 Trajectoire_bezier.h create mode 100644 Trajectoire_circulaire.c create mode 100644 Trajectoire_circulaire.h create mode 100644 Trajectoire_droite.c create mode 100644 Trajectoire_droite.h create mode 100644 Trajet.c create mode 100644 Trajet.h diff --git a/Asser_Position.c b/Asser_Position.c index f6ac6bc..df20315 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -2,8 +2,8 @@ #include "Commande_vitesse.h" #include "math.h" -#define GAIN_P_POSITION 50 -#define GAIN_P_ORIENTATION 500 +#define GAIN_P_POSITION 10 +#define GAIN_P_ORIENTATION 100 struct position_t position_maintien; diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e67593..70aa8b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,11 @@ add_executable(Mon_Projet Localisation.c main.c Temps.c + Trajectoire_bezier.c + Trajectoire_circulaire.c + Trajectoire_droite.c + Trajectoire.c + Trajet.c QEI.c ) diff --git a/Trajectoire.c b/Trajectoire.c new file mode 100644 index 0000000..c099490 --- /dev/null +++ b/Trajectoire.c @@ -0,0 +1,167 @@ +#include "Trajectoire.h" +#include "Trajectoire_bezier.h" +#include "Trajectoire_circulaire.h" +#include "Trajectoire_droite.h" + +#include "math.h" + +#define NB_MAX_TRAJECTOIRES 5 +#define PRECISION_ABSCISSE 0.001f + + +void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon, + float orientation_debut_rad, float 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, float p1_x, float p1_y, float p2_x, float p2_y, + float orientation_debut_rad, float 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, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, + float orientation_debut_rad, float 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_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 +float 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; + + } + return point_xyo; +} + +float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){ + return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) 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 +float 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_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)); + +} diff --git a/Trajectoire.h b/Trajectoire.h new file mode 100644 index 0000000..aecda2b --- /dev/null +++ b/Trajectoire.h @@ -0,0 +1,40 @@ +#ifndef TRAJECTOIRE_H +#define TRAJECTOIRE_H + +enum trajectoire_type_t{ + TRAJECTOIRE_DROITE, + TRAJECTOIRE_CIRCULAIRE, + TRAJECTOIRE_BEZIER, +}; + +struct point_xy_t{ + double x, y; +}; + +struct point_xyo_t{ + struct point_xy_t point_xy; + float orientation; +}; + +struct trajectoire_t { + enum trajectoire_type_t type; + struct point_xy_t p1, p2, p3, p4; + float orientation_debut_rad, orientation_fin_rad; + float rayon, angle_debut_degre, angle_fin_degre; + float longueur; +}; + +float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); +float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse); +float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm); +double distance_points(struct point_xy_t point, struct point_xy_t point_old); +void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, + float rayon, float orientation_debut_rad, float orientation_fin_rad); +void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad); +void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, + float orientation_debut_rad, float orientation_fin_rad); +void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad); +void Trajectoire_inverse(struct trajectoire_t * trajectoire); + +#endif diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c new file mode 100644 index 0000000..5c4aeab --- /dev/null +++ b/Trajectoire_bezier.c @@ -0,0 +1,35 @@ +#include "Trajectoire.h" +#include "Trajectoire_bezier.h" + + +void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){ + struct point_xy_t point, point_old; + float nb_pas=500; + + trajectoire->longueur=0; + point_old = trajectoire->p1; + + for(float abscisse=0; abscisse<=1; abscisse += 1./nb_pas){ + point = Trajectoire_bezier_get_point(trajectoire, abscisse); + trajectoire->longueur += distance_points(point, point_old); + point_old = point; + } +} + + +/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse +/// @param abscisse : compris entre 0 et 1 +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){ + struct point_xy_t point; + point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) + + (double) trajectoire->p4.x * abscisse * abscisse * abscisse; + + point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) + + (double) trajectoire->p4.y * abscisse * abscisse * abscisse; + + return point; +} \ No newline at end of file diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h new file mode 100644 index 0000000..5930684 --- /dev/null +++ b/Trajectoire_bezier.h @@ -0,0 +1,5 @@ +#include "Trajectoire.h" + + +void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse); \ No newline at end of file diff --git a/Trajectoire_circulaire.c b/Trajectoire_circulaire.c new file mode 100644 index 0000000..2ec8726 --- /dev/null +++ b/Trajectoire_circulaire.c @@ -0,0 +1,26 @@ +#include "math.h" +#include "Trajectoire.h" + + +void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){ + float distance_angulaire; + if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){ + distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre; + }else{ + distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre; + } + trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.; +} + +/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse +/// @param abscisse : compris entre 0 et 1 +struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){ + struct point_xy_t point; + float angle_degre; + + angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse; + point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon; + point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon; + + return point; +} diff --git a/Trajectoire_circulaire.h b/Trajectoire_circulaire.h new file mode 100644 index 0000000..6229d88 --- /dev/null +++ b/Trajectoire_circulaire.h @@ -0,0 +1,4 @@ +#include "math.h" + +void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float avancement); diff --git a/Trajectoire_droite.c b/Trajectoire_droite.c new file mode 100644 index 0000000..a2fa2a8 --- /dev/null +++ b/Trajectoire_droite.c @@ -0,0 +1,17 @@ +#include "Trajectoire.h" + + +void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire){ + trajectoire->longueur = distance_points(trajectoire->p1, trajectoire->p2); +} + +/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse +/// @param abscisse : compris entre 0 et 1 +struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse){ + struct point_xy_t point; + + point.x = (float) trajectoire->p1.x * (1. - abscisse) + (float) trajectoire->p2.x * abscisse; + point.y = (float) trajectoire->p1.y * (1. - abscisse) + (float) trajectoire->p2.y * abscisse; + + return point; +} \ No newline at end of file diff --git a/Trajectoire_droite.h b/Trajectoire_droite.h new file mode 100644 index 0000000..e4ed24c --- /dev/null +++ b/Trajectoire_droite.h @@ -0,0 +1,4 @@ +#include "Trajectoire.h" + +void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c new file mode 100644 index 0000000..4f129cd --- /dev/null +++ b/Trajet.c @@ -0,0 +1,216 @@ +#include +#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 +void Trajet_init(){ + Temps_init(); + AsserMoteur_Init(); + 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 +enum etat_trajet_t Trajet_avance(float pas_de_temps_s){ + float 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); + //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; + } + + distance_contrainte_obstacle = Trajet_get_obstacle_mm(); + 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; + } + } + + + // 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; +} \ No newline at end of file diff --git a/Trajet.h b/Trajet.h new file mode 100644 index 0000000..8fd2be8 --- /dev/null +++ b/Trajet.h @@ -0,0 +1,36 @@ +#include "pico/stdlib.h" +#include "Trajectoire.h" + +#ifndef TRAJET_H +#define TRAJET_H + +enum etat_trajet_t{ + TRAJET_EN_COURS, + TRAJET_TERMINE +}; + +// Vitesse et acceleration pour translation pure (en mm/s et mm/s²) +#define TRAJECT_CONFIG_AVANCE_DROIT 1000, 500 +// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²) +#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 +// Vitesse et acceleration - standard (en mm et mm/s²) +#define TRAJECT_CONFIG_STD 500, 500 +// Vitesse et acceleration pour une rotation (rad/s et rad/s²) +#define TRAJECT_CONFIG_ROTATION_PURE 2, 2 + +extern const float distance_pas_obstacle; + +void Trajet_init(); +void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss); +void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); +enum etat_trajet_t Trajet_avance(float temps_s); +struct position_t Trajet_get_consigne(void); +float Trajet_get_obstacle_mm(void); +void Trajet_set_obstacle_mm(float distance_mm); +void Trajet_stop(float); +float Trajet_get_orientation_avance(void); +float Trajet_get_abscisse(); +uint32_t Trajet_get_bloque(); +void Trajet_inverse(); + +#endif diff --git a/main.c b/main.c index 5e81c90..5bd8adc 100644 --- a/main.c +++ b/main.c @@ -9,10 +9,12 @@ #include "hardware/pwm.h" #include "Asser_Position.h" #include "Asser_Moteurs.h" -#include "Localisation.h" #include "Commande_vitesse.h" +#include "Localisation.h" #include "Moteurs.h" #include "Temps.h" +#include "Trajectoire.h" +#include "Trajet.h" #include #include "QEI.h" @@ -28,6 +30,9 @@ uint identifiant_lire(void); uint32_t step_ms=1; float distance1_mm=0, distance2_mm=0; +// DEBUG +extern float abscisse; + void main(void) { int ledpower = 500; @@ -35,11 +40,12 @@ void main(void) stdio_init_all(); - AsserMoteur_Init(); Temps_init(); tension_batterie_init(); identifiant_init(); Localisation_init(); + Trajet_init(); + uint32_t temps_ms = Temps_get_temps_ms(); struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0}; float vitesse_mm_s=100; @@ -51,7 +57,14 @@ void main(void) multicore_launch_core1(affichage); - + + struct trajectoire_t trajectoire; + Trajectoire_droite(&trajectoire, 0, 0, 500, 0, 0, 0); + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + Trajet_debut_trajectoire(trajectoire); + enum etat_trajet_t etat_trajet=TRAJET_EN_COURS; + + sleep_ms(3000); while(1){ @@ -59,15 +72,14 @@ void main(void) temps_ms = Temps_get_temps_ms(); if(temps_ms % step_ms == 0){ QEI_update(); - position_robot.x_mm += temps_ms * vitesse_mm_s / 1000.; - position_robot.y_mm += temps_ms * vitesse_mm_s / 1000.; - Asser_Position(position_robot); - AsserMoteur_Gestion(step_ms); Localisation_gestion(); - } - if(temps_ms % 100 == 0){ - identifiant_lire(); - //printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) ); + + if(etat_trajet != TRAJET_TERMINE){ + AsserMoteur_Gestion(step_ms); + etat_trajet = Trajet_avance((float)step_ms/1000.); + }else{ + Moteur_Stop(); + } } } @@ -83,6 +95,7 @@ void affichage(void){ /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); + printf(">abscisse:%f\n",abscisse); sleep_ms(100); } }