435 lines
15 KiB
C
435 lines
15 KiB
C
#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 distance_bras_correction_mm[6] =
|
|
{
|
|
-12,
|
|
-7,
|
|
-5,
|
|
-15,
|
|
0,
|
|
-25
|
|
};
|
|
|
|
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}, // 1020 : bidouille
|
|
{.x_mm = 1980, .y_mm = 51.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // Attention bidouille !!!
|
|
{.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN},
|
|
{.x_mm = 2970, .y_mm = 1340.8, .angle_radian = 90 * DEGRE_EN_RADIAN} // bidouille : 1386.8 => 1326.8 , 2963.9 => 2970
|
|
};
|
|
|
|
|
|
enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms);
|
|
|
|
/// @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_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_aller_a(
|
|
position_approche_pot.x_mm, position_approche_pot.y_mm,
|
|
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
|
|
if (etat_action == ACTION_TERMINEE){
|
|
etat_attrape_pot = AP_ORIENTE;
|
|
}
|
|
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_PAUSE_DEVANT_OBSTACLE, 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_PAUSE_DEVANT_OBSTACLE, 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_PAUSE_DEVANT_OBSTACLE, 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;
|
|
|
|
}
|
|
|
|
/// @brief Echange les pots avant avec les pots arrière
|
|
/// @param step_ms Le pas de temps
|
|
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
|
|
enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){
|
|
static struct position_t position_initiale;
|
|
struct position_t position_but, position_tmp;
|
|
enum etat_action_t etat_action;
|
|
struct trajectoire_t trajectoire;
|
|
|
|
static int tempo_ms;
|
|
|
|
static enum {
|
|
EPAA_DEPOSE_POT,
|
|
EPAA_TOURNE,
|
|
EPAA_ATTRAPE_POT,
|
|
EPAA_AVANCE_1,
|
|
EPAA_RECULE_1,
|
|
EPAA_RECENTRE,
|
|
EPAA_AVANCE_2,
|
|
EPAA_RECULE_2,
|
|
EPAA_LEVE,
|
|
EPAA_RETOURNE,
|
|
|
|
} etat_echange_pot = EPAA_DEPOSE_POT;
|
|
|
|
switch (etat_echange_pot)
|
|
{
|
|
case EPAA_DEPOSE_POT:
|
|
position_initiale = Localisation_get();
|
|
// On dépose les pots 1, 3, 4 et 6 : 0b00101101 : 0x2D
|
|
if(Strat_2024_depose_pot(0x2D, step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_TOURNE;
|
|
}
|
|
break;
|
|
|
|
case EPAA_TOURNE:
|
|
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
|
|
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().angle_radian, position_initiale.angle_radian - M_PI);
|
|
|
|
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_ATTRAPE_POT;
|
|
position_initiale.angle_radian -= M_PI;
|
|
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
|
|
tempo_ms = 500;
|
|
}
|
|
break;
|
|
|
|
case EPAA_ATTRAPE_POT:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_echange_pot = EPAA_AVANCE_1;
|
|
}
|
|
break;
|
|
|
|
case EPAA_AVANCE_1:
|
|
position_but = Geometrie_deplace(position_initiale, DISTANCE_ECHANGE_POT);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
|
|
step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_RECULE_1;
|
|
}
|
|
break;
|
|
|
|
case EPAA_RECULE_1:
|
|
position_but = Geometrie_deplace(position_initiale, -DISTANCE_ECHANGE_POT);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
|
|
step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_RECENTRE;
|
|
}
|
|
break;
|
|
|
|
case EPAA_RECENTRE:
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
if(Strategie_tourner_et_aller_a(position_initiale.x_mm, position_initiale.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
|
|
step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_AVANCE_2;
|
|
}
|
|
break;
|
|
|
|
case EPAA_AVANCE_2:
|
|
position_tmp = position_initiale;
|
|
position_tmp.angle_radian += 40. * DEGRE_EN_RADIAN ;
|
|
|
|
position_but = Geometrie_deplace(position_tmp, DISTANCE_ECHANGE_POT);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
|
|
step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_RECULE_2;
|
|
}
|
|
break;
|
|
|
|
case EPAA_RECULE_2:
|
|
position_tmp = position_initiale;
|
|
position_tmp.angle_radian += 60. * DEGRE_EN_RADIAN ;
|
|
position_but = Geometrie_deplace(position_tmp, -DISTANCE_ECHANGE_POT);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
|
|
step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_LEVE;
|
|
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(2, BRAS_HAUT, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(3, BRAS_HAUT, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
|
|
tempo_ms = 500;
|
|
}
|
|
break;
|
|
|
|
case EPAA_LEVE:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_echange_pot = EPAA_RETOURNE;
|
|
}
|
|
break;
|
|
|
|
case EPAA_RETOURNE:
|
|
if(Strategie_tourner_a(position_initiale.angle_radian - M_PI, step_ms) == ACTION_TERMINEE){
|
|
etat_echange_pot = EPAA_DEPOSE_POT;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
|
|
default:
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
}
|
|
|
|
/// @brief dépose 1 ou plusieurs pot au pied du robot.
|
|
/// @param masque_pot: choisi les pots à déposer, 0x01 pour le bras 1, 0x02 pour le bras 2, 0x03 pour les bras 1 & 2
|
|
enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
|
|
static int tempo_ms;
|
|
uint8_t masque=1;
|
|
static enum {
|
|
DP_BAISSE_BRAS,
|
|
DP_BAISSE_BRAS_TEMPO,
|
|
DP_RANGE_DOIGT,
|
|
DP_RANGE_BRAS_TEMPO,
|
|
} etat_depose_pot = DP_BAISSE_BRAS;
|
|
switch (etat_depose_pot)
|
|
{
|
|
case DP_BAISSE_BRAS:
|
|
for (int i=0; i< NB_BRAS; i++){
|
|
masque =1;
|
|
masque = masque << i;
|
|
if(masque_pot & masque){
|
|
i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_TIENT);
|
|
}
|
|
}
|
|
tempo_ms=350;
|
|
etat_depose_pot = DP_BAISSE_BRAS_TEMPO;
|
|
break;
|
|
|
|
case DP_BAISSE_BRAS_TEMPO:
|
|
tempo_ms--;
|
|
if(tempo_ms < 0){
|
|
for (int i=0; i< NB_BRAS; i++){
|
|
masque =1;
|
|
masque = masque << i;
|
|
if(masque_pot & masque){
|
|
i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_LACHE);
|
|
}
|
|
}
|
|
tempo_ms=250;
|
|
etat_depose_pot = DP_RANGE_DOIGT;
|
|
}
|
|
break;
|
|
|
|
case DP_RANGE_DOIGT:
|
|
tempo_ms--;
|
|
if(tempo_ms < 0){
|
|
for (int i=0; i< NB_BRAS; i++){
|
|
masque =1;
|
|
masque = masque << i;
|
|
if(masque_pot & masque){
|
|
i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
|
|
tempo_ms=250;
|
|
}
|
|
}
|
|
etat_depose_pot = DP_RANGE_BRAS_TEMPO;
|
|
|
|
}
|
|
break;
|
|
|
|
case DP_RANGE_BRAS_TEMPO:
|
|
tempo_ms--;
|
|
if(tempo_ms < 0){
|
|
etat_depose_pot = DP_BAISSE_BRAS;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
} |