diff --git a/CMakeLists.txt b/CMakeLists.txt index 606e0ce..184ae79 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/Localisation.c b/Localisation.c index 746d72b..83794a8 100644 --- a/Localisation.c +++ b/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" diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c new file mode 100644 index 0000000..f9ad27c --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h new file mode 100644 index 0000000..9867ccc --- /dev/null +++ b/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); diff --git a/Test.c b/Test.c index 35f5568..4a19d80 100644 --- a/Test.c +++ b/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': diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c new file mode 100644 index 0000000..b9d2459 --- /dev/null +++ b/Test_strategie_2024.c @@ -0,0 +1,63 @@ +#include +#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; + +} \ No newline at end of file diff --git a/Test_strategie_2024.h b/Test_strategie_2024.h new file mode 100644 index 0000000..1327ec3 --- /dev/null +++ b/Test_strategie_2024.h @@ -0,0 +1 @@ +int test_strategie_2024(void); \ No newline at end of file diff --git a/Todo b/Todo new file mode 100644 index 0000000..9bad62a --- /dev/null +++ b/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