Ajout de la position des pots
This commit is contained in:
parent
4d251b4b74
commit
32c21947d0
@ -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
|
||||
|
@ -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
45
Strategie_2024_pots.c
Normal 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
16
Strategie_2024_pots.h
Normal 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
11
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':
|
||||
|
63
Test_strategie_2024.c
Normal file
63
Test_strategie_2024.c
Normal 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
1
Test_strategie_2024.h
Normal file
@ -0,0 +1 @@
|
||||
int test_strategie_2024(void);
|
Loading…
Reference in New Issue
Block a user