From ad9de0d3c029addda27bf8355a359929cbd512ec Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 19 Feb 2023 17:56:45 +0100 Subject: [PATCH] =?UTF-8?q?Trajectoires=20avec=20contr=C3=B4le=20de=20la?= =?UTF-8?q?=20vitesse?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 1 + .vscode/settings.json | 5 ++ Localisation.c | 4 +- Moteurs.c | 6 ++ Moteurs.h | 1 + Test.c | 178 +++++++++++++++++++++++++++++++++++++++--- Trajectoire.c | 56 +++++++++++-- Trajectoire.h | 11 ++- Trajet.c | 22 +++--- Trajet.h | 10 ++- gyro.c | 6 +- gyro_ADXRS453.c | 12 +-- 12 files changed, 267 insertions(+), 45 deletions(-) create mode 100644 .gitignore create mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d163863 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build/ \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..e19c2c3 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "timer.h": "c" + } +} \ No newline at end of file diff --git a/Localisation.c b/Localisation.c index db84aa8..b83929f 100644 --- a/Localisation.c +++ b/Localisation.c @@ -21,6 +21,8 @@ void Localisation_gestion(){ double distance_roue_c_mm = QEI_get_mm(QEI_C_NAME); double delta_x_ref_robot, delta_y_ref_robot; + double old_orientation_radian = position.angle_radian; + delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm) / 3.0; delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm) * RACINE_DE_3 / 3.0; @@ -31,7 +33,7 @@ void Localisation_gestion(){ position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM); } - // Projection dans le référentiel du robot + // Projection dans le référentiel de la table position.x_mm += delta_x_ref_robot * cos(position.angle_radian) - delta_y_ref_robot * sin(position.angle_radian); position.y_mm += delta_x_ref_robot * sin(position.angle_radian) + delta_y_ref_robot * cos(position.angle_radian); diff --git a/Moteurs.c b/Moteurs.c index 3b4b901..d4a5480 100644 --- a/Moteurs.c +++ b/Moteurs.c @@ -100,4 +100,10 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){ break; } +} + +void Moteur_Stop(void){ + Moteur_SetVitesse(MOTEUR_A, 0); + Moteur_SetVitesse(MOTEUR_B, 0); + Moteur_SetVitesse(MOTEUR_C, 0); } \ No newline at end of file diff --git a/Moteurs.h b/Moteurs.h index 7af1ea1..1eed7da 100644 --- a/Moteurs.h +++ b/Moteurs.h @@ -11,3 +11,4 @@ enum t_moteur { void Moteur_Init(void); void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ); +void Moteur_Stop(void); diff --git a/Test.c b/Test.c index 78a34db..112b750 100644 --- a/Test.c +++ b/Test.c @@ -46,6 +46,8 @@ void affiche_localisation(void); int test_i2c_lecture_pico_annex(); int test_i2c_lecture_pico_annex_nb(); int test_i2c_lecture_pico_annex_nb2(); +int test_aller_retour(); +void test_trajectoire_teleplot(); // Mode test : renvoie 0 pour quitter le mode test @@ -62,8 +64,9 @@ int mode_test(){ printf("H - Asser Position - avance\n"); printf("I - Asser Position - avance et tourne (gyro)\n"); printf("J - Asser Position - avance et tourne (sans gyro)\n"); - printf("M - pour les moteurs\n"); + printf("K - Trajets aller retour avec Gyro\n"); printf("L - pour la localisation\n"); + printf("M - pour les moteurs\n"); printf("T - Trajectoire\n"); printf("U - Scan du bus i2c\n"); printf("V - APDS_9960\n"); @@ -123,15 +126,21 @@ int mode_test(){ while(test_asser_position_avance_et_tourne(0)); break; - case 'M': - case 'm': - while(test_moteurs()); + case 'K': + case 'k': + while(test_aller_retour()); break; + case 'L': case 'l': while(test_localisation()); break; + case 'M': + case 'm': + while(test_moteurs()); + break; + case 'T': case 't': while(test_trajectoire()); @@ -434,6 +443,104 @@ void test_trajectoire_printf(){ } +void test_trajectoire_teleplot(){ + struct position_t _position, _consigne; + _consigne = Trajet_get_consigne(); + while(1){ + _consigne = Trajet_get_consigne(); + _position = Localisation_get(); + uint32_t temps; + temps = time_us_32()/1000; + printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); + printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); + printf(">Position:%f:%f|xy\n>Consigne_Position:%f:%f|xy\n", _position.x_mm, _position.y_mm, _consigne.x_mm, _consigne.y_mm); + } + +} + +int test_aller_retour(){ + int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; + Trajet_init(); + struct trajectoire_t trajectoire; + printf("Choix trajectoire :\n"); + printf("B - Bezier\n"); + printf("C - Circulaire\n"); + printf("D - Droite\n"); + printf("E - Avance et tourne\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); + printf("Trajectoire de Bézier\n"); + break; + + case 'c': + case 'C': + Trajectoire_circulaire(&trajectoire, 0, 350, -90, 90, 350); + printf("Trajectoire circulaire\n"); + break; + + case 'd': + case 'D': + Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + printf("Trajectoire droite\n"); + break; + + case 'e': + case 'E': + Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + trajectoire.orientation_debut_rad = 0; + trajectoire.orientation_fin_rad = -M_PI; + printf("Trajectoire droite avec rotation\n"); + break; + + default: return 0; + } + + printf("Init gyroscope\n"); + Gyro_Init(); + //printf("C'est parti !\n"); + stdio_flush(); + + set_position_avec_gyroscope(1); + + Trajet_debut_trajectoire(trajectoire); + multicore_launch_core1(test_trajectoire_teleplot); + do{ + // Routines à 1 ms + QEI_update(); + Localisation_gestion(); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + Gyro_Read(_step_ms_gyro); + } + + if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){ + Trajectoire_inverse(&trajectoire); + Trajet_debut_trajectoire(trajectoire); + }else{ + AsserMoteur_Gestion(_step_ms); + } + sleep_ms(_step_ms); + temps_ms += _step_ms; + lettre = getchar_timeout_us(0); + //lettre = PICO_ERROR_TIMEOUT; + }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + printf("Lettre : %d; %c\n", lettre, lettre); + + Moteur_Stop(); + + return 0; + +} + + + int test_trajectoire(){ int lettre, _step_ms = 1, temps_ms=0; Trajet_init(); @@ -450,23 +557,28 @@ int test_trajectoire(){ case 'b': case 'B': Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0); + printf("Trajectoire Bezier\n"); break; case 'c': case 'C': Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250); + printf("Trajectoire circulaire\n"); break; case 'd': case 'D': Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + printf("Trajectoire droite\n"); break; default: return 0; } + sleep_ms(3000); + Trajet_debut_trajectoire(trajectoire); - multicore_launch_core1(test_trajectoire_printf); + multicore_launch_core1(test_trajectoire_teleplot); do{ // Routines à 1 ms QEI_update(); @@ -482,6 +594,7 @@ int test_trajectoire(){ sleep_ms(_step_ms); temps_ms += _step_ms; lettre = getchar_timeout_us(0); + lettre = PICO_ERROR_TIMEOUT; }while(lettre == PICO_ERROR_TIMEOUT); return 0; @@ -501,9 +614,11 @@ int test_asser_position_avance_et_tourne(int m_gyro){ position_consigne.y_mm = 0; printf("Le robot avance à 100 mm/s\n"); - printf("Init gyroscope\n"); - Gyro_Init(); - printf("C'est parti !\n"); + if(m_gyro){ + printf("Init gyroscope\n"); + Gyro_Init(); + printf("C'est parti !\n"); + } stdio_flush(); set_position_avec_gyroscope(m_gyro); @@ -731,17 +846,58 @@ int test_QIE_mm(){ int test_localisation(){ int lettre; struct position_t position; + uint32_t temps_ms; + uint32_t _step_ms_gyro = 2, _step_ms=1; + uint32_t m_gyro = 0; + + printf("A - Sans gyroscope\n"); + printf("B - Avec Gyroscope\n"); + do{ + lettre = getchar_timeout_us(TEST_TIMEOUT_US); + stdio_flush(); + }while(lettre == PICO_ERROR_TIMEOUT); + + switch(lettre){ + case 'A': + case 'a': + set_position_avec_gyroscope(0); + printf("Sans gyroscope\n"); + break; + case 'B': + case 'b': + set_position_avec_gyroscope(1); + printf("Avec gyroscope, initialisation...\n"); + m_gyro=1; + Gyro_Init(); + break; + default: + return 0; + } + + temps_ms = Temps_get_temps_ms(); + + multicore_launch_core1(affiche_localisation); + + printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n"); do{ + while(temps_ms == Temps_get_temps_ms()); QEI_update(); + if(m_gyro){ + if(temps_ms % _step_ms_gyro == 0){ + Gyro_Read(_step_ms_gyro); + } + } Localisation_gestion(); position = Localisation_get(); - printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654); - sleep_ms(100); lettre = getchar_timeout_us(0); - }while(lettre == PICO_ERROR_TIMEOUT); + + temps_ms += _step_ms; + }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); + + multicore_reset_core1(); return 0; diff --git a/Trajectoire.c b/Trajectoire.c index 4b42233..269d239 100644 --- a/Trajectoire.c +++ b/Trajectoire.c @@ -17,6 +17,8 @@ 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; } void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y){ @@ -26,6 +28,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double trajectoire->p2.x = p2_x; trajectoire->p2.y = p2_y; trajectoire->longueur = -1; + trajectoire->orientation_debut_rad = 0; + trajectoire->orientation_fin_rad = 0; } 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){ @@ -39,6 +43,36 @@ 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; +} + +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 @@ -68,21 +102,29 @@ double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ /// @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){ +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: - return Trajectoire_droite_get_point(trajectoire, abscisse); + point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse); + point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse); break; case TRAJECTOIRE_CIRCULAIRE: - return Trajectoire_circulaire_get_point(trajectoire, abscisse); + point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse); + point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse); break; case TRAJECTOIRE_BEZIER: - return Trajectoire_bezier_get_point(trajectoire, abscisse); + point_xyo.point_xy = Trajectoire_bezier_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 @@ -97,14 +139,14 @@ double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, d } // 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) ); + 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), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) ); + 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; } diff --git a/Trajectoire.h b/Trajectoire.h index 39d5d56..344b199 100644 --- a/Trajectoire.h +++ b/Trajectoire.h @@ -11,20 +11,27 @@ struct point_xy_t{ double x, y; }; +struct point_xyo_t{ + struct point_xy_t point_xy; + double orientation; +}; + struct trajectoire_t { enum trajectoire_type_t type; struct point_xy_t p1, p2, p3, p4; - double orientation_debut, orientation_fin; + double orientation_debut_rad, orientation_fin_rad; 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); +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); +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_inverse(struct trajectoire_t * trajectoire); #endif diff --git a/Trajet.c b/Trajet.c index e65e657..28ffbdd 100644 --- a/Trajet.c +++ b/Trajet.c @@ -25,14 +25,15 @@ void Trajet_init(){ void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){ abscisse = 0; vitesse_mm_s = 0; - acceleration_mm_ss = 1500; + acceleration_mm_ss = 500; position_mm = 0; trajet_trajectoire = trajectoire; } -int Trajet_avance(double pas_de_temps_s){ - double distance_mm, orientation_radian; - struct point_xy_t point; +enum trajet_etat_t Trajet_avance(double pas_de_temps_s){ + double distance_mm; + enum trajet_etat_t trajet_etat = TRAJET_EN_COURS; + struct point_xyo_t point; struct position_t position; // Calcul de la vitesse @@ -48,20 +49,17 @@ int Trajet_avance(double pas_de_temps_s){ // 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.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(abscisse >= 1 ){ - return TRAJET_TERMINE; + trajet_etat = TRAJET_TERMINE; } - return TRAJET_EN_COURS; + return trajet_etat; } diff --git a/Trajet.h b/Trajet.h index d38f7f8..7b27692 100644 --- a/Trajet.h +++ b/Trajet.h @@ -1,8 +1,12 @@ -#define TRAJET_EN_COURS 0 -#define TRAJET_TERMINE 1 +#include "Trajectoire.h" + +enum trajet_etat_t{ + TRAJET_EN_COURS, + TRAJET_TERMINE +}; void Trajet_init(); void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); -int Trajet_avance(double temps_s); +enum trajet_etat_t Trajet_avance(double temps_s); struct position_t Trajet_get_consigne(void); diff --git a/gyro.c b/gyro.c index 2b1c0ee..f5bad2d 100644 --- a/gyro.c +++ b/gyro.c @@ -60,7 +60,7 @@ void Gyro_Init(void){ //uint speed = spi_init(spi0, 10 * 1000); // SPI init @ 10 kHz uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz - printf("vitesse SPI : %d\n", speed); + //printf("vitesse SPI : %d\n", speed); spi_set_format(spi0, 8, SPI_CPHA_0, SPI_CPOL_0, SPI_MSB_FIRST); @@ -70,9 +70,9 @@ void Gyro_Init(void){ puts("Gyroscope non trouve"); while(1); // On s'arrête là ! }else{ - puts("Gyroscope trouve"); + //puts("Gyroscope trouve"); if(!gyro_config()){ - puts("gyro_config ok !"); + //puts("gyro_config ok !"); }else{ puts("gyro_config FAILED !"); diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 80d3943..6252ddb 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -76,7 +76,7 @@ int gyro_init_check(){ // On suit les instructions de la page 20 de la fiche technique sleep_ms(100); // init du gyro - On ignore la réponse - printf("T=100ms\n"); + //printf("T=100ms\n"); tampon_envoi[0] = 0x30; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00; @@ -88,7 +88,7 @@ int gyro_init_check(){ sleep_ms(50); // t=150ms - On ignore, les données ne sont pas actualisées - printf("T=150ms\n"); + //printf("T=150ms\n"); tampon_envoi[0] = 0x30; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00; @@ -103,7 +103,7 @@ int gyro_init_check(){ affiche_tampon_32bits(tampon_reception); sleep_ms(50); // t=200ms - En cours d'autotest - printf("T=200ms\n"); + //printf("T=200ms\n"); tampon_envoi[0] = 0x30; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00; @@ -118,7 +118,7 @@ int gyro_init_check(){ affiche_tampon_32bits(tampon_reception); sleep_us(1); // t=200ms + TD - résultats de 200ms + TD, en cours d'autotest. - printf("T=200ms+TD\n"); + //printf("T=200ms+TD\n"); tampon_envoi[0] = 0x30; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00; @@ -130,10 +130,10 @@ int gyro_init_check(){ printf("Gyro_Init - SQ bits (%#01x)!= 0x4", Gyro_SensorData.SQ); return 1; } - affiche_tampon_32bits(tampon_reception); + //affiche_tampon_32bits(tampon_reception); sleep_us(1); // t=200ms + 2TD - doit être nominal - printf("T=200ms+2TD\n"); + //printf("T=200ms+2TD\n"); tampon_envoi[0] = 0x00; tampon_envoi[1] = 0x00; tampon_envoi[2] = 0x00;