Déplacement des tests des fonctions déplacements (pas encore 100% testé) + réalisation des fonctions I2C pour piloter les actionneurs des pots

Geometrie_robot.h

@@ -1,4 +1,4 @@
-#define DISTANCE_ROUES_CENTRE_MM 84.25
+#define DISTANCE_ROUES_CENTRE_MM 112
 
 // Distance entre le centre du robot et un angle du robot
 #define RAYON_ROBOT 125

QEI.c

@@ -86,8 +86,8 @@ void QEI_update(void){
 }
 
 /// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente
-/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation.
+/// Les signes sont inversés (sauf A) car le réducteur inverse le sens de rotation.
-/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique
+/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électronique
 /// @param  qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
 /// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update()
 int QEI_get(enum QEI_name_t qei){

Readme.md

@@ -78,3 +78,37 @@ Le fichier [i2c_maitre.c](i2c_maitre.c) propose une implémentation non-bloquant
 - La fonction *i2c_transmission* (ou une fonction englobant celle-ci, telle que *i2c_lire_registre_nb*) doit être appelée jusqu'à ce qu'elle renvoie I2C_SUCCES. 
 
 Pour un exemple concret lisant une valeur dans une mémoire i2c, voir *test_i2c_lecture_pico_annex_nb2* de [Test.c](Test.c)
+
+Communication
+=====================
+
+Carte Servomoteur
+-----------------
+
+Discussion en I2C avec la carte servomoteur, celle-ci étant esclave.
+
+Registres:
+
+Adresse, RW, Fonction
+* 0x00 (R) : Position du bras 1
+* 0x01 (R) : Position du bras 2
+* 0x02 (R) : Position du bras 3
+* 0x03 (R) : Position du bras 4
+* 0x04 (R) : Position du bras 5
+* 0x05 (R) : Position du bras 5
+* 0x06 (RW) : Action demandée / action en cours
+
+   'G' : Demande d'attraper plante et dépose à gauche
+
+   'g' : Action en cours: attraper plante et dépose à gauche
+
+   'D' : Demande d'attraper plante et dépose à droite
+
+   'd' : Action en cours: attraper plante et dépose à droite
+
+   't' : Action terminée
+
+Position du bras : 1 octet, découpé ainsi :
+
+- 4 premiers bits : position de l'aimant
+- 4 derniers bits : hauteur du bras

Test.c

@@ -42,21 +42,14 @@
 #define TEST_TIMEOUT_US 10000000
 
 int test_APDS9960(void);
-int test_localisation(void);
 int test_asser_position_avance(void);
 int test_asser_position_avance_et_tourne(int, int);
 int test_transition_gyro_pas_gyro(void);
-int test_trajectoire(void);
 void affiche_localisation(void);
-int test_aller_retour();
-int test_endurance_aller_retour();
-void test_trajectoire_teleplot();
 int test_capteurs_balise(void);
 int test_geometrie(void);
 int test_angle_balise(void);
 
-int continuous_printf = 1;
-
 
 // Mode test : renvoie 0 pour quitter le mode test
 int mode_test(){
@@ -69,8 +62,6 @@ 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("K - Trajets aller retour avec Gyro\n");
-    printf("L - pour la localisation\n");
     printf("N - Fonctions geometrique\n");
     printf("O - Analyse obstacle\n");
     printf("P - Asser Position - perturbation\n");
@@ -121,15 +112,6 @@ int mode_test(){
         while(test_asser_position_avance_et_tourne(0, 0));
         break;
 
-    case 'K':
-    case 'k':
-        while(test_aller_retour());
-        break;
-
-    case 'L':
-    case 'l':
-        while(test_localisation());
-        break;
 
     case 'N':
     case 'n':
@@ -156,11 +138,6 @@ int mode_test(){
         while(test_log());
         break;
 
-    case 'T':
-    case 't':
-        while(test_trajectoire());
-        break;
-
     case 'U':
     case 'u':
         while(test_i2c());
@@ -171,11 +148,6 @@ int mode_test(){
         while(test_APDS9960());
         break;
     
-    case 'W':
-    case 'w':
-        while(test_endurance_aller_retour());
-        break;
-    
     case 'Z':
     case 'z':
         while(Demonstration_menu());
@@ -255,357 +227,6 @@ int test_APDS9960(){
     return 1;
 }
 
-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;
-        if(continuous_printf){
-            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:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps);
-            printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME));
-            printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
-        }
-        Log_gestion();
-    }
-
-}
-
-int test_aller_retour(){
-    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
-    const float corr_angle = 1.145;
-    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 (ok)\n");
-    printf("F - Avance et tourne (Nok)\n");
-    printf("G - Avance (Calibre angle)\n");
-    printf("R - Rotation pure\n");
-    do{
-         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
-         stdio_flush();
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    switch(lettre){
-        case 'b':
-        case 'B':
-            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':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            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, 0, 0);
-            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
-            printf("Trajectoire droite\n");
-            break;
-
-        case 'e':
-        case 'E':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            Trajectoire_droite(&trajectoire, 0, 0, 0, 1500, 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 'g':
-        case 'G':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            Trajectoire_droite(&trajectoire, 0, 0, 
-                2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)),
-                0, 0);
-            printf("Trajectoire droite pour calibration angle de départ\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 rotation pure\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);
-    temps_ms = Temps_get_temps_ms();
-    do{
-        // Routines à 1 ms
-        while(temps_ms == Temps_get_temps_ms());
-        temps_ms = Temps_get_temps_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);
-        }
-        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();
-    multicore_reset_core1();
-
-    return 0;
-    
-}
-
-/// @brief Fonction de test d'endurance, notamment pour valider le bon fonctionnement du gyroscope 
-/// sur le long terme et en dynamique. Tente aussi de valider les enregistrements des messages de log.
-/// Pour l'instant aucune erreur enregistrée, donc pas hyper probant.
-/// @return 
-int test_endurance_aller_retour(){
-    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
-    const float corr_angle = 1.145;
-    Trajet_init();
-    Log_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 (ok)\n");
-    printf("F - Avance et tourne (Nok)\n");
-    printf("G - Avance (Calibre angle)\n");
-    printf("R - Rotation pure\n");
-    do{
-         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
-         stdio_flush();
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    switch(lettre){
-        case 'b':
-        case 'B':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
-            Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
-            Log_message("Trajectoire de Bézier\n", INFO);
-            break;
-
-        case 'c':
-        case 'C':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            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, 0, 0);
-            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
-            printf("Trajectoire droite\n");
-            break;
-
-        case 'e':
-        case 'E':
-            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 'g':
-        case 'G':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            Trajectoire_droite(&trajectoire, 0, 0, 
-                2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)),
-                0, 0);
-            printf("Trajectoire droite pour calibration angle de départ\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");
-            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);
-    temps_ms = Temps_get_temps_ms();
-    int nb_aller=0;
-    do{
-        // Routines à 1 ms
-        while(temps_ms == Temps_get_temps_ms());
-        temps_ms = Temps_get_temps_ms();
-        QEI_update();
-        Localisation_gestion();
-
-        // Routine à 2 ms
-        if(temps_ms % _step_ms_gyro == 0){
-            Gyro_Read(_step_ms_gyro);
-        }
-
-        // Routine à 100 ms
-        if(temps_ms % 100 == 0){
-            char message[256];
-            uint32_t temps;
-            struct position_t _position, _consigne;
-            temps = time_us_32()/1000;
-            _consigne = Trajet_get_consigne();
-            _position = Localisation_get();
-            
-            /*sprintf(message, ">X:%ld:%.2f\n>Y:%ld:%.2f\n>orientation:%ld:%.2f", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180);
-            Log_message(message, TELEPLOT);
-            sprintf(message, ">C_X:%ld:%.2f\n>C_Y:%ld:%.2f\n>C_orientation:%ld:%.2f", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180);
-            Log_message(message, TELEPLOT);
-            sprintf(message, ">V_A:%ld:%.2f\n>V_B:%ld:%.2f\n>V_C:%ld:%.2f", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME));
-            Log_message(message, TELEPLOT);
-            sprintf(message, ">Vc_A:%ld:%.2f\n>Vc_B:%ld:%.2f\n>Vc_C:%ld:%.2f", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
-            Log_message(message, TELEPLOT);*/
-        
-        }
-        
-        if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
-            char message[256];
-            nb_aller++;
-            Trajectoire_inverse(&trajectoire);
-            Trajet_debut_trajectoire(trajectoire);
-            if(nb_aller % 2){
-                sprintf(message, "Aller %d\n", nb_aller / 2 +1);
-            }else{
-                sprintf(message, "retour %d\n", nb_aller / 2);
-            }
-            Log_message(message, INFO);
-        }else{
-            AsserMoteur_Gestion(_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();
-    continuous_printf = 0;
-    printf("Boucle Log\n");
-    do{
-        Log_gestion();
-        lettre = getchar_timeout_us(0);
-        if(lettre == 'L'){
-            Log_get_full_log();
-        }
-    }while(lettre != 'q');
-    printf("Fin boucle Log\n");
-
-    return 0;
-    
-}
-
-
-
-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, 0, 0);
-            printf("Trajectoire Bezier\n");
-            break;
-
-        case 'c':
-        case 'C':
-            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, 0, 0);
-            printf("Trajectoire droite\n");
-            break;
-        
-        default: return 0;
-    }
-
-    sleep_ms(3000);
-
-    Trajet_debut_trajectoire(trajectoire);
-    multicore_launch_core1(test_trajectoire_teleplot);
-    do{
-        // Routines à 1 ms
-        QEI_update();
-        Localisation_gestion();
-        
-        if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
-            Moteur_SetVitesse(MOTEUR_A, 0);
-            Moteur_SetVitesse(MOTEUR_B, 0);
-            Moteur_SetVitesse(MOTEUR_C, 0);
-        }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);
-
-    return 0;
-    
-}
-
 
 /// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
 /// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans

Test_i2c.c

@@ -290,7 +290,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){
     enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
 
 
-    
+    printf("A - Aimant tient pot\n");
+    printf("C - Aimant lache pot\n");
     printf("D - Deguisement On\n");
     printf("E - Deguisement Off\n");
     printf("F - Ferme porte\n");
@@ -321,6 +322,22 @@ int test_i2c_ecriture_pico_annex_nb_2(){
         if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){
             printf("lettre !\n");
             switch(lettre){
+                case 'a':
+                case 'A':
+                    for(int i=1; i<7; i++){
+                        i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
+                    }
+                    printf("=> Lache pot\n");
+                    break;
+
+                case 'c':
+                case 'C':
+                    for(int i=1; i<7; i++){
+                        i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_TIENT);
+                    }
+                    printf("=> Tient pot\n");
+                    break;
+
                 case 'd':
                 case 'D':
                     i2c_annexe_active_deguisement();

Tests_deplacement.c

@@ -2,11 +2,18 @@
 #include <math.h>
 #include "pico/multicore.h"
 
-#include "QEI.h"
-#include "Moteurs.h"
 #include "Asser_Moteurs.h"
-#include "Geometrie_robot.h"
 #include "Commande_vitesse.h"
+#include "Geometrie_robot.h"
+#include "gyro.h"
+#include "Localisation.h"
+#include "Log.h"
+#include "Moteurs.h"
+#include "QEI.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+#include "Temps.h"
+#include "Robot_config.h"
 
 int test_avance(void);
 int test_cde_vitesse(void);
@@ -15,6 +22,12 @@ int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
 int test_cde_vitesse_rectangle(void);
 int test_cde_vitesse_cercle(void);
 
+int test_trajectoire(void);
+int test_aller_retour(void);
+int test_endurance_aller_retour(void);
+
+bool continuous_printf=true;
+
 #define TEST_TIMEOUT_US 10000000
 
 // Mode test : renvoie 0 pour quitter le mode test
@@ -23,6 +36,9 @@ int mode_test_deplacement(){
     printf("Appuyez sur une touche pour entrer en mode test :\n");
     printf("A - AsserMoteur - pour avancer selon Y\n");
     printf("B - AsserMoteur - Commande en vitesse...\n");
+    printf("C - Trajectoires - sans gyroscope...\n");
+    printf("D - Aller-retour - avec gyroscope...\n");
+    printf("D - Aller-retour - Endurance - avec gyroscope...\n");
     
     
     stdio_flush();
@@ -39,6 +55,21 @@ int mode_test_deplacement(){
         while(test_cde_vitesse());
         break;
 
+    case 'c':
+    case 'C':
+        while(test_trajectoire());
+        break;
+
+    case 'd':
+    case 'D':
+        while(test_aller_retour());
+        break;
+
+    case 'e':
+    case 'E':
+        while(test_endurance_aller_retour());
+        break;
+
     case PICO_ERROR_TIMEOUT:
         iteration--;        
         if(iteration == 0){
@@ -205,3 +236,353 @@ int test_cde_vitesse_cercle(){
 
     return 0;
 }
+
+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;
+        if(continuous_printf){
+            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:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps);
+            printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME));
+            printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
+        }
+        Log_gestion();
+    }
+
+}
+
+/// @brief Lance le robot sur une trajectoire (Bezier, circulaire ou droite)
+/// Localisation sans Gyroscope
+/// @return 
+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, 0, 0);
+            printf("Trajectoire Bezier\n");
+            break;
+
+        case 'c':
+        case 'C':
+            Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
+            printf("Trajectoire circulaire\n");
+            break;
+
+        case 'd':
+        case 'D':
+            Trajectoire_droite(&trajectoire, 0, 0, 700, 0, 0, 0);
+            printf("Trajectoire droite\n");
+            break;
+        
+        default: return 0;
+    }
+
+    Trajet_debut_trajectoire(trajectoire);
+    multicore_launch_core1(test_trajectoire_teleplot);
+    do{
+        // Routines à 1 ms
+        QEI_update();
+        Localisation_gestion();
+        
+        if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
+            Moteur_SetVitesse(MOTEUR_A, 0);
+            Moteur_SetVitesse(MOTEUR_B, 0);
+            Moteur_SetVitesse(MOTEUR_C, 0);
+        }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);
+
+    return 0;
+    
+}
+
+int test_aller_retour(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
+    const float corr_angle = 1.145;
+    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 (ok)\n");
+    printf("F - Avance et tourne (Nok)\n");
+    printf("G - Avance (Calibre angle)\n");
+    printf("R - Rotation pure\n");
+    do{
+         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    switch(lettre){
+        case 'b':
+        case 'B':
+            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':
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            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, 0, 0);
+            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
+            printf("Trajectoire droite\n");
+            break;
+
+        case 'e':
+        case 'E':
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            Trajectoire_droite(&trajectoire, 0, 0, 0, 1500, 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 'g':
+        case 'G':
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            Trajectoire_droite(&trajectoire, 0, 0, 
+                2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)),
+                0, 0);
+            printf("Trajectoire droite pour calibration angle de départ\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 rotation pure\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);
+    temps_ms = Temps_get_temps_ms();
+    do{
+        // Routines à 1 ms
+        while(temps_ms == Temps_get_temps_ms());
+        temps_ms = Temps_get_temps_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);
+        }
+        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();
+    multicore_reset_core1();
+
+    return 0;
+    
+}
+
+/// @brief Fonction de test d'endurance, notamment pour valider le bon fonctionnement du gyroscope 
+/// sur le long terme et en dynamique. Tente aussi de valider les enregistrements des messages de log.
+/// Pour l'instant aucune erreur enregistrée, donc pas hyper probant.
+/// @return 
+int test_endurance_aller_retour(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
+    const float corr_angle = 1.145;
+    Trajet_init();
+    Log_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 (ok)\n");
+    printf("F - Avance et tourne (Nok)\n");
+    printf("G - Avance (Calibre angle)\n");
+    printf("R - Rotation pure\n");
+    do{
+         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    switch(lettre){
+        case 'b':
+        case 'B':
+            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
+            Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
+            Log_message("Trajectoire de Bézier\n", INFO);
+            break;
+
+        case 'c':
+        case 'C':
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            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, 0, 0);
+            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
+            printf("Trajectoire droite\n");
+            break;
+
+        case 'e':
+        case 'E':
+            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 'g':
+        case 'G':
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            Trajectoire_droite(&trajectoire, 0, 0, 
+                2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)),
+                0, 0);
+            printf("Trajectoire droite pour calibration angle de départ\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");
+            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);
+    temps_ms = Temps_get_temps_ms();
+    int nb_aller=0;
+    do{
+        // Routines à 1 ms
+        while(temps_ms == Temps_get_temps_ms());
+        temps_ms = Temps_get_temps_ms();
+        QEI_update();
+        Localisation_gestion();
+
+        // Routine à 2 ms
+        if(temps_ms % _step_ms_gyro == 0){
+            Gyro_Read(_step_ms_gyro);
+        }
+
+        // Routine à 100 ms
+        if(temps_ms % 100 == 0){
+            char message[256];
+            uint32_t temps;
+            struct position_t _position, _consigne;
+            temps = time_us_32()/1000;
+            _consigne = Trajet_get_consigne();
+            _position = Localisation_get();
+            
+            /*sprintf(message, ">X:%ld:%.2f\n>Y:%ld:%.2f\n>orientation:%ld:%.2f", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180);
+            Log_message(message, TELEPLOT);
+            sprintf(message, ">C_X:%ld:%.2f\n>C_Y:%ld:%.2f\n>C_orientation:%ld:%.2f", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180);
+            Log_message(message, TELEPLOT);
+            sprintf(message, ">V_A:%ld:%.2f\n>V_B:%ld:%.2f\n>V_C:%ld:%.2f", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME));
+            Log_message(message, TELEPLOT);
+            sprintf(message, ">Vc_A:%ld:%.2f\n>Vc_B:%ld:%.2f\n>Vc_C:%ld:%.2f", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
+            Log_message(message, TELEPLOT);*/
+        
+        }
+        
+        if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
+            char message[256];
+            nb_aller++;
+            Trajectoire_inverse(&trajectoire);
+            Trajet_debut_trajectoire(trajectoire);
+            if(nb_aller % 2){
+                sprintf(message, "Aller %d\n", nb_aller / 2 +1);
+            }else{
+                sprintf(message, "retour %d\n", nb_aller / 2);
+            }
+            Log_message(message, INFO);
+        }else{
+            AsserMoteur_Gestion(_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();
+    continuous_printf = 0;
+    printf("Boucle Log\n");
+    do{
+        Log_gestion();
+        lettre = getchar_timeout_us(0);
+        if(lettre == 'L'){
+            Log_get_full_log();
+        }
+    }while(lettre != 'q');
+    printf("Fin boucle Log\n");
+
+    return 0;
+    
+}

Trajet.c

@@ -3,7 +3,9 @@
 #include "Trajectoire.h"
 #include "Trajet.h"
 #include "Asser_Position.h"
+#include "Asser_Moteurs.h"
 #include "Monitoring.h"
+#include "Temps.h"
 
 float Trajet_calcul_vitesse(float temps_s);
 int Trajet_terminee(float abscisse);
@@ -25,6 +27,8 @@ 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;

i2c_annexe.c

@@ -16,52 +16,73 @@
 #define TAILLE_DONNEES_EMISSION 6
 #define TAILLE_DONNEES_RECEPTION 15
 
-
+#define ADRESSE_PIC18F4550 0x31
+#define ADRESSE_PIC18F4550_DEBUT_W 0x00
+#define TAILLE_DONNEES_PIC18F4550_EMISSION 7
 
 uint8_t donnees_emission[TAILLE_DONNEES_EMISSION];
+uint8_t donnees_emission_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION];
 uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION];
 
+
 uint donnees_a_envoyer=0;
+uint donnees_a_envoyer_pic=0;
 
 void i2c_annexe_gestion(){
     static enum {
-        EMISSION_DONNEES,
+        EMISSION_DONNEES_RPi,
-        EMISSION_TEMPO,
+        EMISSION_TEMPO_RPi,
-        RECEPTION_DONNEES
+        RECEPTION_DONNEES_RPi,
-    } etat_i2c_annexe=EMISSION_DONNEES;
+        EMISSION_PIC18F,
+    } etat_i2c_annexe=EMISSION_PIC18F;
 
     enum i2c_resultat_t retour_i2c;
     static uint32_t temps;
     const uint32_t tempo = 1000;
 
     switch(etat_i2c_annexe){
-        case EMISSION_DONNEES:
+        case EMISSION_DONNEES_RPi:
             if(donnees_a_envoyer){
                 retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_W, donnees_emission, TAILLE_DONNEES_EMISSION);
                 if(retour_i2c == I2C_SUCCES){
-                    etat_i2c_annexe = EMISSION_TEMPO;
+                    etat_i2c_annexe = EMISSION_TEMPO_RPi;
                     donnees_a_envoyer=0;
                     temps = time_us_32();
                 }
             }else{
-                etat_i2c_annexe = EMISSION_TEMPO;
+                etat_i2c_annexe = EMISSION_TEMPO_RPi;
                 temps = time_us_32();
             }
             break;
         
-        case EMISSION_TEMPO:
+        case EMISSION_TEMPO_RPi:
             if(temps + tempo < time_us_32() ){
-                etat_i2c_annexe = RECEPTION_DONNEES;
+                etat_i2c_annexe = RECEPTION_DONNEES_RPi;
             }
             break;
         
-        case RECEPTION_DONNEES:
+        case RECEPTION_DONNEES_RPi:
             retour_i2c = i2c_lire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_R, donnees_reception, TAILLE_DONNEES_RECEPTION);
             if(retour_i2c == I2C_SUCCES){
-                etat_i2c_annexe = EMISSION_DONNEES;
+                etat_i2c_annexe = EMISSION_PIC18F;
                 temps = time_us_32();
             }
             break;
+
+        case EMISSION_PIC18F:
+            if(donnees_a_envoyer_pic){
+                retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PIC18F4550, ADRESSE_PIC18F4550_DEBUT_W, 
+                                donnees_emission_pic18f4550, TAILLE_DONNEES_PIC18F4550_EMISSION);
+                if(retour_i2c == I2C_SUCCES){
+                    etat_i2c_annexe = EMISSION_DONNEES_RPi;
+                    temps = time_us_32();
+                    donnees_a_envoyer_pic=0;
+                }
+            }else{
+                etat_i2c_annexe = EMISSION_DONNEES_RPi;
+            }
+            break;
+
     }
 
 }
@@ -153,3 +174,16 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
         distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R];
     }
 }
+
+/// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs
+/// @param actionneur de 1 à 6, pour le "bras" correspondant".
+/// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h
+/// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h
+void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){
+    if(actionneur < 1 || actionneur > 6){
+        printf("Erreur: i2c_annexe_actionneur_pot\n" );
+        return;
+    }
+    donnees_emission_pic18f4550[actionneur-1] = (pos_bras << 2) | pos_doigt;
+    donnees_a_envoyer_pic=1;
+}

i2c_annexe.h

@@ -3,6 +3,16 @@
 #define CONTACTEUR_ACTIF 0
 #define CONTACTEUR_INACTIF 1
 
+#define DOIGT_TIENT 1
+#define DOIGT_LACHE 0
+
+#define BRAS_PLIE 0
+#define BRAS_POT_SOL 1
+#define BRAS_POT_POT 2
+#define BRAS_ECARTE 3
+#define BRAS_DEPOSE_JARDINIERE 4
+#define BRAS_HAUT 5
+
 void i2c_annexe_gestion(void);
 void i2c_annexe_active_turbine(void);
 void i2c_annexe_desactive_turbine(void);
@@ -32,3 +42,5 @@ void i2c_annexe_plie_bras(void);
 void i2c_annexe_deplie_bras(void);
 
 uint8_t i2c_annexe_get_tension_batterie(void);
+
+void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt);