From e1d3568f53402d11ff1caaffe2a6b6a074d99431 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 10 Dec 2022 15:29:04 +0100 Subject: [PATCH] =?UTF-8?q?Gestion=20des=20d=C3=A9placements=20suivants=20?= =?UTF-8?q?des=20trajectoires=20avec=20un=20contr=C3=B4le=20en=20vitesse.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Asser_Position.h | 1 + CMakeLists.txt | 5 ++ Geometrie.h | 9 +++ Localisation.h | 5 +- Trajectoire.c | 117 +++++++++++++++++++++++++++++++++++++++ Trajectoire.h | 30 ++++++++++ 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 | 94 +++++++++++++++++++++++++++++++ Trajet.h | 5 ++ test.c | 84 +++++++++++++++++++++++++--- 15 files changed, 428 insertions(+), 13 deletions(-) create mode 100644 Geometrie.h 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.h b/Asser_Position.h index 772bdad..6451fd4 100644 --- a/Asser_Position.h +++ b/Asser_Position.h @@ -1 +1,2 @@ +#include "Geometrie.h" void Asser_Position(struct position_t position_consigne); \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index bf79063..42a02d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,11 @@ gyro_ADXRS453.c Localisation.c Moteurs.c Temps.c +Trajet.c +Trajectoire.c +Trajectoire_bezier.c +Trajectoire_circulaire.c +Trajectoire_droite.c Servomoteur.c spi_nb.c) diff --git a/Geometrie.h b/Geometrie.h new file mode 100644 index 0000000..d13fa64 --- /dev/null +++ b/Geometrie.h @@ -0,0 +1,9 @@ +#ifndef GEOMETRIE_H +#define GEOMETRIE_H + +struct position_t{ + double x_mm, y_mm; + double angle_radian; +}; + +#endif diff --git a/Localisation.h b/Localisation.h index 0b4f726..cab2c97 100644 --- a/Localisation.h +++ b/Localisation.h @@ -1,7 +1,4 @@ -struct position_t{ - double x_mm, y_mm; - double angle_radian; -}; +#include "Geometrie.h" struct position_t Localisation_get(void); void Localisation_gestion(); diff --git a/Trajectoire.c b/Trajectoire.c new file mode 100644 index 0000000..4b42233 --- /dev/null +++ b/Trajectoire.c @@ -0,0 +1,117 @@ +#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.001 + + +void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon){ + 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; +} + +void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y){ + 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; +} + +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){ + 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; +} + +/// @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_xy_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){ + switch(trajectoire->type){ + case TRAJECTOIRE_DROITE: + return Trajectoire_droite_get_point(trajectoire, abscisse); + break; + + case TRAJECTOIRE_CIRCULAIRE: + return Trajectoire_circulaire_get_point(trajectoire, abscisse); + break; + + case TRAJECTOIRE_BEZIER: + return Trajectoire_bezier_get_point(trajectoire, abscisse); + break; + } + +} + +/// @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 et les trajectoires circulaires + delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire); + delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) ); + + // 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), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) ); + 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..39d5d56 --- /dev/null +++ b/Trajectoire.h @@ -0,0 +1,30 @@ +#ifndef TRAJECTOIRE_H +#define TRAJECTOIRE_H + +enum trajectoire_type_t{ + TRAJECTOIRE_DROITE, + TRAJECTOIRE_CIRCULAIRE, + TRAJECTOIRE_BEZIER +}; + +struct point_xy_t{ + double x, y; +}; + +struct trajectoire_t { + enum trajectoire_type_t type; + struct point_xy_t p1, p2, p3, p4; + double orientation_debut, orientation_fin; + double rayon, angle_debut_degre, angle_fin_degre; + double longueur; +}; + +double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); +double 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, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon); +void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y); +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); + +#endif diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c new file mode 100644 index 0000000..0fb7628 --- /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; + double nb_pas=500; + + trajectoire->longueur=0; + point_old = trajectoire->p1; + + for(double 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..d190ec7 --- /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){ + double 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, double abscisse){ + struct point_xy_t point; + double angle_degre; + + angle_degre = (double) trajectoire->angle_debut_degre * (1-abscisse) + (double) 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..80366f4 --- /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, double avancement); diff --git a/Trajectoire_droite.c b/Trajectoire_droite.c new file mode 100644 index 0000000..74e3529 --- /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, double abscisse){ + struct point_xy_t point; + + point.x = (double) trajectoire->p1.x * (1. - abscisse) + (double) trajectoire->p2.x * abscisse; + point.y = (double) trajectoire->p1.y * (1. - abscisse) + (double) 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..4937c55 --- /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, double abscisse); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c new file mode 100644 index 0000000..e1a62c8 --- /dev/null +++ b/Trajet.c @@ -0,0 +1,94 @@ +#include +#include "Geometrie.h" +#include "Trajectoire.h" +#include "Asser_Position.h" + +#define VITESSE_MAX_MM_S 1000 + +double Trajet_calcul_vitesse(double temps_s); + +double abscisse; +double position_mm; // Position en mm sur la trajectoire +double vitesse_mm_s; +double acceleration_mm_ss; +struct trajectoire_t trajet_trajectoire; +struct position_t position_consigne; + +void Trajet_init(){ + abscisse = 0; + vitesse_mm_s = 0; + acceleration_mm_ss = 300; + position_mm = 0; +} + +void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){ + abscisse = 0; + vitesse_mm_s = 0; + acceleration_mm_ss = 1500; + position_mm = 0; + trajet_trajectoire = trajectoire; +} + +void Trajet_avance(double pas_de_temps_s){ + double distance_mm, orientation_radian; + struct point_xy_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); + + // Obtention de l'orientation consigne + orientation_radian = 0; // TODO + + position.x_mm = point.x; + position.y_mm = point.y; + position.angle_radian = orientation_radian; + + position_consigne=position; + Asser_Position(position); + +} + +struct position_t Trajet_get_consigne(){ + return position_consigne; +} + +/// @brief Calcule la vitesse à partir de l'accelé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_MM_S){ + vitesse = VITESSE_MAX_MM_S; + } + return vitesse; +} \ No newline at end of file diff --git a/Trajet.h b/Trajet.h new file mode 100644 index 0000000..0036f04 --- /dev/null +++ b/Trajet.h @@ -0,0 +1,5 @@ + +void Trajet_init(); +void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); +void Trajet_avance(double temps_s); +struct position_t Trajet_get_consigne(void); diff --git a/test.c b/test.c index b6fe905..09e8f53 100644 --- a/test.c +++ b/test.c @@ -6,16 +6,17 @@ #include "math.h" #include "gyro.h" -#include "Temps.h" -#include "spi_nb.h" -#include "Servomoteur.h" +#include "Asser_Moteurs.h" +#include "Asser_Position.h" +#include "Commande_vitesse.h" +#include "Localisation.h" #include "Moteurs.h" #include "QEI.h" -#include "Asser_Moteurs.h" -#include "Localisation.h" -#include "Commande_vitesse.h" -#include "Asser_Position.h" - +#include "Servomoteur.h" +#include "spi_nb.h" +#include "Temps.h" +#include "Trajectoire.h" +#include "Trajet.h" const uint LED_PIN = 25; const uint LED_PIN_ROUGE = 28; @@ -38,6 +39,7 @@ int test_cde_vitesse_rectangle(); int test_cde_vitesse_cercle(); int test_asser_position_avance(); int test_asser_position_avance_et_tourne(); +int test_trajectoire(); void affiche_localisation(); int main() { @@ -148,6 +150,7 @@ int mode_test(){ printf("I - Asser Position - avance et tourne\n"); printf("M - pour les moteurs\n"); printf("L - pour la localisation\n"); + printf("T - Trajectoire\n"); stdio_flush(); int rep = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); @@ -204,9 +207,14 @@ int mode_test(){ break; case 'L': case 'l': - /* code */ while(test_localisation()); break; + + case 'T': + case 't': + while(test_trajectoire()); + break; + case PICO_ERROR_TIMEOUT: iteration--; if(iteration == 0){ @@ -222,6 +230,64 @@ int mode_test(){ } +void test_trajectoire_printf(){ + struct position_t _position; + while(1){ + _position = Trajet_get_consigne(); + printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180); + } + +} + +int test_trajectoire(){ + int lettre, _step_ms = 1, temps_ms=0; + Trajet_init(); + struct trajectoire_t trajectoire; + printf("Choix trajectoire :\n"); + printf("B - Bezier\n"); + printf("C - Circulaire\n"); + printf("D - Droite\n"); + do{ + lettre = getchar_timeout_us(TEST_TIMEOUT_US); + stdio_flush(); + }while(lettre == PICO_ERROR_TIMEOUT); + switch(lettre){ + case 'b': + case 'B': + Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0); + break; + + case 'c': + case 'C': + Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250); + break; + + case 'd': + case 'D': + Trajectoire_droite(&trajectoire, 0, 0, 0, 500); + break; + + default: return 0; + } + + Trajet_debut_trajectoire(trajectoire); + multicore_launch_core1(test_trajectoire_printf); + do{ + // Routines à 1 ms + QEI_update(); + Localisation_gestion(); + AsserMoteur_Gestion(_step_ms); + + Trajet_avance(_step_ms/1000.); + sleep_ms(_step_ms); + temps_ms += _step_ms; + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + + return 0; + +} + int test_asser_position_avance_et_tourne(){ int lettre, _step_ms = 1, temps_ms=0; struct position_t position_consigne;