Ajout de la position des pots

CMakeLists.txt

@@ -34,12 +34,14 @@ Strategie.c
 Strategie_deplacement.c
 Strategie_prise_cerises.c
 Strategie_pousse_gateau.c
+Strategie_2024_pots.c
 Temps.c
 Test.c
 Test_gyro.c
 Test_i2c.c
 Test_log.c
 Test_strategie.c
+Test_strategie_2024.c
 Tests_deplacement.c
 Tests_unitaires.c
 Trajet.c

Localisation.c

@@ -1,5 +1,6 @@
 #include "gyro.h"
 #include "Localisation.h"
+#include "Temps.h"
 #include "QEI.h"
 #include "math.h"
 #include "Geometrie_robot.h"

Strategie_2024_pots.c

@@ -0,0 +1,45 @@
+#include "math.h"
+#include "Strategie_2024_pots.h"
+
+struct position_t position_pots_dans_groupe_pot[5] =
+    {
+        {.x_mm = -40, .y_mm = 69.2, .angle_radian = 120 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 40, .y_mm = 69.2, .angle_radian = 60 * DEGRE_EN_RADIAN},
+        {.x_mm = -80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN},
+        {.x_mm = 80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN},
+        {.x_mm = 0, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN}
+    };
+
+struct position_t position_groupe_pot[6] =
+    {
+        {.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 1000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 2000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN}
+    };
+
+
+/// @brief renvoie la position du centre du pot ainsi que l'ange par lequel l'attraper
+/// @param groupe_pot Position du groupe de pot
+/// @param num_pot Pot à prendre, entre 0 et 4 (ou utiliser les macros POT_x)
+struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot){
+    struct position_t position_pot;
+    struct position_t my_position_groupe_pot;
+    
+    my_position_groupe_pot = position_groupe_pot[groupe_pot];
+    
+    float angle_groupe_pot = my_position_groupe_pot.angle_radian;
+    position_pot.x_mm = my_position_groupe_pot.x_mm + 
+        cosf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].x_mm -
+        sinf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].y_mm;
+
+    position_pot.y_mm = my_position_groupe_pot.y_mm + 
+        sinf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].x_mm +
+        cosf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].y_mm;
+    
+    position_pot.angle_radian = my_position_groupe_pot.angle_radian + angle_groupe_pot;
+
+    return position_pot;
+}

Strategie_2024_pots.h

@@ -0,0 +1,16 @@
+#include "Geometrie.h"
+
+#define POT_1 0
+#define POT_2 1
+#define POT_3 2
+#define POT_4 3
+#define POT_5 4
+
+#define GROUPE_POT_L1 0
+#define GROUPE_POT_L2 1
+#define GROUPE_POT_B1 2
+#define GROUPE_POT_B2 3
+#define GROUPE_POT_R2 4
+#define GROUPE_POT_R1 5
+
+struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot);

Test.c

@@ -34,6 +34,7 @@
 #include "Test_i2c.h"
 #include "Test_log.h"
 #include "Test_strategie.h"
+#include "Test_strategie_2024.h"
 #include "Tests_unitaires.h"
 #include "Tests_deplacement.h"
 #include "Test.h"
@@ -57,7 +58,8 @@ int mode_test(){
     printf("Appuyez sur une touche pour entrer en mode test :\n");
     printf("A - Tests unitaires...\n");
     printf("B - Tests deplacement...\n");
-    printf("F - Strategie...\n");
+    printf("E - Strategie...\n");
+    printf("F - Strategie 2024...\n");
     printf("G - Lecture des capteurs\n");
     printf("H - Asser Position - avance\n");
     printf("I - Asser Position - avance et tourne (gyro)\n");
@@ -87,9 +89,14 @@ int mode_test(){
         while(mode_test_deplacement());
         break;
 
+    case 'E':
+    case 'e':
+        while(test_strategie());
+        break;
+
     case 'F':
     case 'f':
-        while(test_strategie());
+        while(test_strategie_2024());
         break;
 
     case 'G':

Test_strategie_2024.c

@@ -0,0 +1,63 @@
+#include <stdio.h>
+#include "pico/error.h"
+#include "Geometrie.h"
+#include "Strategie_2024_pots.h"
+
+int test_calcul_position_pot(void);
+
+int test_strategie_2024(){
+    printf("A - Position groupes pot.\n");
+    
+    int lettre;
+    do{
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+    switch(lettre){
+        case 'a':
+        case 'A':
+            while(test_calcul_position_pot());
+            break;
+    }
+}
+
+void print_position(struct position_t position){
+    printf("x_mm: %.2f, y_mm: %.2f, angle: %.2f\n", position.x_mm, position.y_mm, position.angle_radian/DEGRE_EN_RADIAN);
+}
+
+int test_calcul_position_pot(){
+    printf("\ngroupe: GROUPE_POT_B1, pot: 5 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_B1, POT_5));
+
+    printf("\ngroupe: GROUPE_POT_B2, pot: 1 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_B2, POT_1));
+
+    printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));
+
+    printf("\ngroupe: GROUPE_POT_L2, pot: 1 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L2, POT_1));
+
+    printf("\ngroupe: GROUPE_POT_R1, pot: 1 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_R1, POT_1));
+
+    printf("\ngroupe: GROUPE_POT_R2, pot: 1 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_R2, POT_1));
+
+    printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));
+
+    printf("\ngroupe: GROUPE_POT_L1, pot: 2 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_2));
+
+    printf("\ngroupe: GROUPE_POT_L1, pot: 3 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_3));
+
+    printf("\ngroupe: GROUPE_POT_L1, pot: 4 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_4));
+
+    printf("\ngroupe: GROUPE_POT_L1, pot: 5 \n");
+    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_5));
+
+    return 0;
+
+}

Test_strategie_2024.h

@@ -0,0 +1 @@
+int test_strategie_2024(void);

Todo

@@ -0,0 +1,5 @@
+Réussir à faire tourner le robot sur lui-même en mode démo => OK
+Doigt 3 : Ne lache pas bien le pot. Revoir le fil de fer
+
+Arrêter proprement les logs en cas d'erreur
+Changer l'architecture du programme, mettre les fonctions cycliques sur le core1