73 lines
2.9 KiB
C
73 lines
2.9 KiB
C
#include "math.h"
|
|
#include "Strategie.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "Strategie_2024_pots.h"
|
|
|
|
#define DISTANCE_APPROCHE_POT
|
|
|
|
float angle_bras[6] =
|
|
{
|
|
180 * DEGRE_EN_RADIAN,
|
|
120 * DEGRE_EN_RADIAN,
|
|
60 * DEGRE_EN_RADIAN,
|
|
0,
|
|
-60 * DEGRE_EN_RADIAN,
|
|
-120 * DEGRE_EN_RADIAN
|
|
};
|
|
|
|
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;
|
|
}
|
|
|
|
/// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots
|
|
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){
|
|
// Parcourir la trajectoire pour aller jusqu'au premier pot
|
|
|
|
// Pour chaque pot
|
|
// Baisser le bras correspondant
|
|
// Aller jusqu'au point de prise de pot en s'orientant pour prendre le pot
|
|
// Avancer de X cm en direction du pot
|
|
// Reculer dans l'axe de prise et rejoindre le point de prise suivant
|
|
// Pendant le mouvement, apres 1 sec (à confirmer) Lever le bras
|
|
|
|
}
|