Trajectoires avec contrôle de la vitesse

.gitignore

@@ -0,0 +1 @@
+build/

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "timer.h": "c"
+    }
+}

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);
 

Moteurs.c

@@ -101,3 +101,9 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
     }
 
 }
+
+void Moteur_Stop(void){
+    Moteur_SetVitesse(MOTEUR_A, 0);
+    Moteur_SetVitesse(MOTEUR_B, 0);
+    Moteur_SetVitesse(MOTEUR_C, 0);
+}

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);

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 'K':
-    case 'm':
+    case 'k':
-        while(test_moteurs());
+        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");
+    if(m_gyro){
-    Gyro_Init();
+        printf("Init gyroscope\n");
-    printf("C'est parti !\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;
     

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;
     }
     

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

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){
+enum trajet_etat_t Trajet_avance(double pas_de_temps_s){
-    double distance_mm, orientation_radian;
+    double distance_mm;
-    struct point_xy_t point;
+    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
+    position.x_mm = point.point_xy.x;
-    orientation_radian = 0; // TODO
+    position.y_mm = point.point_xy.y;
-
+    position.angle_radian = point.orientation;
-    position.x_mm = point.x;
-    position.y_mm = point.y;
-    position.angle_radian = orientation_radian;
 
     position_consigne=position;
     Asser_Position(position);
 
     if(abscisse >= 1 ){
-        return TRAJET_TERMINE;
+        trajet_etat = TRAJET_TERMINE;
     }
-    return TRAJET_EN_COURS;
+    return trajet_etat;
 
 }
 

Trajet.h

@@ -1,8 +1,12 @@
-#define TRAJET_EN_COURS 0
+#include "Trajectoire.h"
-#define TRAJET_TERMINE  1
+
+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);

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 !");
 

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;