Ajout de la position des pots

This commit is contained in:
Samuel 2024-03-26 20:46:33 +01:00
parent 4d251b4b74
commit 32c21947d0
8 changed files with 142 additions and 2 deletions

View File

@ -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

View File

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

45
Strategie_2024_pots.c Normal file
View File

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

16
Strategie_2024_pots.h Normal file
View File

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

11
Test.c
View File

@ -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':

63
Test_strategie_2024.c Normal file
View File

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

1
Test_strategie_2024.h Normal file
View File

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

5
Todo Normal file
View File

@ -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