#include "math.h"
#include "Strategie.h"
#include "Geometrie_robot.h"
#include "Commande_vitesse.h"
#include "Strategie_2024_pots.h"
#include "i2c_annexe.h"
#include "Localisation.h"
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
};
float angle_bras_correction[6] =
{
0 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0,
0 * DEGRE_EN_RADIAN,
7 * DEGRE_EN_RADIAN
};
float distance_bras_correction_mm[6] =
{
0,
0,
-10,
-15,
0,
0
};
enum etat_bras_t{
BRAS_LIBRE,
BRAS_OCCUPE,
} etat_bras[NB_BRAS];
struct position_t position_pots_dans_groupe_pot[5] =
{
{.x_mm = -40, .y_mm = 69.2, .angle_radian = -60 * DEGRE_EN_RADIAN},
{.x_mm = 40, .y_mm = 69.2, .angle_radian = -120 * 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 = 1020, .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 = position_pots_dans_groupe_pot[num_pot].angle_radian + angle_groupe_pot;
return position_pot;
}
/// @brief A Affiner
/// @return numéro du bras libre (entre 0 et 5)
int ordre_bras[NB_BRAS]={5, 0, 1, 2, 3, 4};
int get_bras_libre(void){
for (int i=0; i<NB_BRAS; i++){
if(etat_bras[ordre_bras[i]] == BRAS_LIBRE){
return ordre_bras[i];
}
}
return 9;
}
int ordre_pot[5]={POT_1, POT_2, POT_4, POT_5, POT_3};
int get_pot_suivant(int pot){
for(int i=0; i<4; i++){
if(ordre_pot[i] == pot){
return ordre_pot[++i];
}
}
return POT_INVALIDE;
}
/// @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, uint32_t step_ms){
// Parcourir la trajectoire pour aller jusqu'au premier pot
static struct position_t position_pot, position_approche_pot, position_attrape_pot;
enum etat_action_t etat_action;
enum validite_vl53l8_t validite;
float distance, angle;
static int bras, tempo_ms;
// Pour le 1er pot
static int pot = POT_1;
static enum {
AP_ALLER_VERS_GROUPE_POT,
AP_RECALE,
AP_ORIENTE,
AP_APPROCHE_POT,
AP_ATTRAPE_POT,
AP_RETOUR_ET_LEVE_POT,
AP_FINALISE
} etat_attrape_pot = AP_ALLER_VERS_GROUPE_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
switch (etat_attrape_pot)
{
case AP_ALLER_VERS_GROUPE_POT:
bras = get_bras_libre();
position_pot = groupe_pot_get_pot(groupe_pot, POT_1);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
/*etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0],
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN),
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_RECALE;
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
}
break;
case AP_RECALE:
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_DISTANCE_LOIN){
if(fabs(distance + DISTANCE_CENTRE_CAPTEUR - Localisation_get().x_mm) < 25){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
commande_vitesse_stop();
//if(couleur == COULEUR_BLEU){
// Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
/*}else{
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
}*/
etat_attrape_pot = AP_ORIENTE;
}else{
//printf("Erreur - recalage trop loin\n");
}
}
break;
case AP_ORIENTE:
bras = get_bras_libre();
if(Strategie_tourner_a(position_approche_pot.angle_radian - angle_bras[bras], step_ms) == ACTION_TERMINEE){
etat_attrape_pot = AP_ATTRAPE_POT;
}
break;
case AP_APPROCHE_POT:
bras = get_bras_libre();
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
/*etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
SANS_EVITEMENT, step_ms);*/
etat_action = Strategie_aller_a_puis_tourner(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
etat_attrape_pot = AP_ATTRAPE_POT;
}
break;
case AP_ATTRAPE_POT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
etat_action = Strategie_tourner_et_aller_a(
position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
tempo_ms=250;
etat_attrape_pot = AP_RETOUR_ET_LEVE_POT;
etat_bras[bras] = BRAS_OCCUPE;
}
break;
case AP_RETOUR_ET_LEVE_POT:
if(tempo_ms >= 0){
tempo_ms -= step_ms;
}else{
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
}
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_APPROCHE_POT;
pot = get_pot_suivant(pot);
if(pot > 4){
tempo_ms=250;
etat_attrape_pot = AP_FINALISE;
break;
}
position_pot = groupe_pot_get_pot(groupe_pot, pot);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
}
break;
case AP_FINALISE:
if(tempo_ms >= 0){
tempo_ms -= step_ms;
}else{
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
}
etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
EVITEMENT_SANS_EVITEMENT, step_ms);
if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
return ACTION_TERMINEE;
}
break;
default:
break;
}
return ACTION_EN_COURS;
}