diff --git a/CMakeLists.txt b/CMakeLists.txt index f76e32d..3356661 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ Trajectoire.c Trajectoire_bezier.c Trajectoire_circulaire.c Trajectoire_droite.c +Trajectoire_rotation.c Servomoteur.c spi_nb.c) diff --git a/Readme.md b/Readme.md index d52cd5e..350c4c2 100644 --- a/Readme.md +++ b/Readme.md @@ -58,7 +58,7 @@ Asservissement en position Gestion des trajets ------------------- -Cette fonction permet de parcourir une trajectoire en tenant compte des contraintes de vitesse. +Cette fonction permet de parcourir une trajectoire en tenant compte des contraintes de vitesse. Elle calcule la vitesse désirée et l'avancement désiré du robot sur la trajectoire (en millimètre). Elle délègue ensuite aux fonctions des trajectoires le calcul du point consigne qui sera envoyé à l'asservissement en position. 1. Calcul de la vitesse - Prise en compte de l'accélération maximale - Prise en compte de la décélération pour s'arrêter. Voir [cet article](http://poivron-robotique.fr/Consigne-de-vitesse.html). diff --git a/Test.c b/Test.c index d28f948..668a207 100644 --- a/Test.c +++ b/Test.c @@ -261,7 +261,7 @@ int test_i2c_lecture_pico_annex_nb(){ time_i2c[0] = time_us_32(); - // On charge l'adresse de l'escalve + // On charge l'adresse de l'esclave i2c0->hw->enable = 0; i2c0->hw->tar = adresse; i2c0->hw->enable = 1; @@ -400,7 +400,6 @@ int test_i2c_bus(){ ret = PICO_ERROR_GENERIC; else ret = i2c_read_blocking(i2c_default, addr, &rxdata, 1, false); - //ret = i2c_write_blocking(i2c_default, addr, &rxdata, 1, false); printf(ret < 0 ? "." : "@"); printf(addr % 16 == 15 ? "\n" : " "); @@ -468,7 +467,9 @@ int test_aller_retour(){ printf("B - Bezier\n"); printf("C - Circulaire\n"); printf("D - Droite\n"); - printf("E - Avance et tourne\n"); + printf("E - Avance et tourne (ok)\n"); + printf("F - Avance et tourne (Nok)\n"); + printf("R - Rotation pure\n"); do{ lettre = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); @@ -476,25 +477,42 @@ int test_aller_retour(){ switch(lettre){ case 'b': case 'B': - Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0); + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0); printf("Trajectoire de Bézier\n"); break; case 'c': case 'C': - Trajectoire_circulaire(&trajectoire, 0, 700, -90, 90, 350); + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + Trajectoire_circulaire(&trajectoire, 0, 700, -90, 90, 350, 0, 0); printf("Trajectoire circulaire\n"); break; case 'd': case 'D': - Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0); + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); printf("Trajectoire droite\n"); break; case 'e': case 'E': - Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); + printf("Trajectoire droite avec rotation (OK)\n"); + break; + + case 'f': + case 'F': + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); + printf("Trajectoire droite avec rotation (Nok)\n"); + break; + + case 'r': + case 'R': + Trajectoire_rotation(&trajectoire, 0, 0, 0, 700); trajectoire.orientation_debut_rad = 0; trajectoire.orientation_fin_rad = M_PI; printf("Trajectoire droite avec rotation\n"); @@ -560,19 +578,19 @@ int test_trajectoire(){ switch(lettre){ case 'b': case 'B': - Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0); + Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0); printf("Trajectoire Bezier\n"); break; case 'c': case 'C': - Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250); + Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); printf("Trajectoire circulaire\n"); break; case 'd': case 'D': - Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0); printf("Trajectoire droite\n"); break; diff --git a/Trajectoire.c b/Trajectoire.c index 269d239..09fdc0d 100644 --- a/Trajectoire.c +++ b/Trajectoire.c @@ -2,6 +2,7 @@ #include "Trajectoire_bezier.h" #include "Trajectoire_circulaire.h" #include "Trajectoire_droite.h" +#include "Trajectoire_rotation.h" #include "math.h" @@ -9,7 +10,8 @@ #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){ +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; @@ -17,22 +19,24 @@ void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, trajectoire->angle_fin_degre = angle_fin_degre; trajectoire->rayon = rayon; trajectoire->longueur = -1; - trajectoire->orientation_debut_rad = 0; - trajectoire->orientation_fin_rad = 0; + 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){ +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 = 0; - trajectoire->orientation_fin_rad = 0; + 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){ +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; @@ -43,8 +47,17 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double trajectoire->p4.x = p4_x; trajectoire->p4.y = p4_y; trajectoire->longueur = -1; - trajectoire->orientation_debut_rad = 0; - trajectoire->orientation_fin_rad = 0; + 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){ @@ -119,6 +132,11 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou 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; } @@ -137,8 +155,12 @@ double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, d if(distance_mm == 0){ return abscisse; } - // Ceci permet d'avoir une abscisse exact sur les trajectoires droites et les trajectoires circulaires + // 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 diff --git a/Trajectoire.h b/Trajectoire.h index 344b199..8791967 100644 --- a/Trajectoire.h +++ b/Trajectoire.h @@ -4,7 +4,8 @@ enum trajectoire_type_t{ TRAJECTOIRE_DROITE, TRAJECTOIRE_CIRCULAIRE, - TRAJECTOIRE_BEZIER + TRAJECTOIRE_BEZIER, + TRAJECTOIRE_ROTATION, }; struct point_xy_t{ @@ -29,9 +30,12 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou double Trajectoire_get_orientation_rad(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); +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); +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); +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); +void Trajectoire_rotation(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double orientation_debut_rad, double orientation_fin_rad); void Trajectoire_inverse(struct trajectoire_t * trajectoire); #endif diff --git a/Trajectoire_rotation.c b/Trajectoire_rotation.c new file mode 100644 index 0000000..dc0d3de --- /dev/null +++ b/Trajectoire_rotation.c @@ -0,0 +1,16 @@ +#include "Trajectoire_rotation.h" + +// ATTENTION, alors que toutes les autres trajectoires se déplacent +// en distance en mètre avec une vitesse en mètres/seconde, ici, il s'agit de radian et de radian/seconde. + +void Trajectoire_rotation_get_longueur(struct trajectoire_t * trajectoire){ + trajectoire->longueur = trajectoire->orientation_fin_rad - trajectoire->orientation_debut_rad; + if(trajectoire->longueur < 0){ + trajectoire->longueur = -trajectoire->longueur; + } +} + +// Le robot reste sur place +struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, double abscisse){ + return trajectoire->p1; +} \ No newline at end of file diff --git a/Trajectoire_rotation.h b/Trajectoire_rotation.h new file mode 100644 index 0000000..0c932bc --- /dev/null +++ b/Trajectoire_rotation.h @@ -0,0 +1,4 @@ +#include "Trajectoire.h" + +void Trajectoire_rotation_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, double abscisse); diff --git a/Trajet.c b/Trajet.c index 527d691..62fea98 100644 --- a/Trajet.c +++ b/Trajet.c @@ -9,7 +9,7 @@ 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=1000; +double vitesse_max_trajet_mm_s=500; double acceleration_mm_ss; struct trajectoire_t trajet_trajectoire; struct position_t position_consigne; diff --git a/Trajet.h b/Trajet.h index fdea91d..fee1d5a 100644 --- a/Trajet.h +++ b/Trajet.h @@ -5,6 +5,13 @@ enum trajet_etat_t{ 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 pour une rotation (rad/s et rad/s²) +#define TRAJECT_CONFIG_ROTATION_PURE 2, 2 + void Trajet_init(); void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss); diff --git a/doc/ProgrammeHolonome2023.png b/doc/ProgrammeHolonome2023.png index 02f9f8e..e1d881a 100644 Binary files a/doc/ProgrammeHolonome2023.png and b/doc/ProgrammeHolonome2023.png differ diff --git a/doc/ProgrammeHolonome2023.svg b/doc/ProgrammeHolonome2023.svg index 804b53c..0305c09 100644 --- a/doc/ProgrammeHolonome2023.svg +++ b/doc/ProgrammeHolonome2023.svg @@ -7,9 +7,9 @@ xmlns="http://www.w3.org/2000/svg" xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" - width="1163.9067mm" - height="339.91183mm" - viewBox="0 0 1163.9067 339.91183" + width="1173.7749mm" + height="338.62747mm" + viewBox="0 0 1173.7749 338.62747" version="1.1" id="svg5" inkscape:version="1.0.2 (e86c870879, 2021-01-15)" @@ -39,9 +39,9 @@ inkscape:pagecheckerboard="0" inkscape:document-units="mm" showgrid="false" - inkscape:zoom="0.54838704" - inkscape:cx="3048.8334" - inkscape:cy="1048.0463" + inkscape:zoom="0.32793568" + inkscape:cx="1221.6943" + inkscape:cy="150.91722" inkscape:window-width="1920" inkscape:window-height="1016" inkscape:window-x="0" @@ -55,12 +55,68 @@ inkscape:snap-nodes="false" inkscape:snap-others="false"> + + + + + + + + + + + + + inkscape:isstock="true" + inkscape:collect="always"> - - - + transform="translate(429.86237,83.924844)"> @@ -572,31 +614,6 @@ y="30.357273" rx="4.640729" ry="4.640729" /> - Trajectoiredroite - - - - - - - - - + + Trame SPI Trajectoire.c - + Trajectoiredroite + + Trajectoire_droite.c + id="text3115">Trajectoire_droite.c + + transform="translate(-190.94094,3.591109)"> + transform="translate(-127.43216,61.952822)"> du robot) + + Rotation pure + + Trajectoire_rotation.c + + + + + + +