97 lines
3.4 KiB
C
97 lines
3.4 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"
|
|
#include "Score.h"
|
|
|
|
enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms);
|
|
|
|
/// @brief Fonction demande au robot d'aller activer les panneaux solaires
|
|
enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms){
|
|
enum etat_action_t etat_action;
|
|
struct trajectoire_t trajectoire;
|
|
static int tempo_ms;
|
|
|
|
static enum {
|
|
TP_APPROCHE_PANNEAU_1, // Position et orientation pour attaquer le panneau 1, bras 2 qui pousse
|
|
TP_TOURNE_PANNEAU_1,
|
|
TP_TOURNE_PANNEAU_2,
|
|
TP_TOURNE_PANNEAU_3,
|
|
TP_TOURNE_PANNEAU_4,
|
|
TP_TOURNE_PANNEAU_5,
|
|
TP_TOURNE_PANNEAU_6,
|
|
TP_DEGAGEMENT,
|
|
TP_DEGAGEMENT_ACTIF
|
|
} etat_tourne_panneaux = TP_TOURNE_PANNEAU_1;
|
|
|
|
|
|
switch (etat_tourne_panneaux)
|
|
{
|
|
case TP_TOURNE_PANNEAU_1:
|
|
if(couleur == COULEUR_BLEU){
|
|
etat_action = Strategie_aller_a(315, 170, EVITEMENT_SANS_EVITEMENT, step_ms);
|
|
}else{
|
|
etat_action = Strategie_aller_a(3000 - 305, 170, EVITEMENT_SANS_EVITEMENT, step_ms);
|
|
}
|
|
if (etat_action == ACTION_TERMINEE){
|
|
etat_tourne_panneaux = TP_TOURNE_PANNEAU_2;
|
|
}
|
|
break;
|
|
|
|
case TP_TOURNE_PANNEAU_2:
|
|
if(couleur == COULEUR_BLEU){
|
|
etat_action = active_panneau_solaire(562, step_ms);
|
|
}else{
|
|
etat_action = active_panneau_solaire(3000 - 558, step_ms);
|
|
}
|
|
if (etat_action == ACTION_TERMINEE){
|
|
etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
|
|
}else if(etat_action == ACTION_ECHEC){
|
|
etat_tourne_panneaux = TP_DEGAGEMENT;
|
|
}
|
|
break;
|
|
|
|
case TP_TOURNE_PANNEAU_3:
|
|
if(couleur == COULEUR_BLEU){
|
|
etat_action = active_panneau_solaire(795, step_ms);
|
|
}else{
|
|
etat_action = active_panneau_solaire(3000 - 802, step_ms);
|
|
}
|
|
if (etat_action == ACTION_TERMINEE){
|
|
etat_tourne_panneaux = TP_DEGAGEMENT;
|
|
}else if(etat_action == ACTION_ECHEC){
|
|
etat_tourne_panneaux = TP_DEGAGEMENT;
|
|
}
|
|
break;
|
|
|
|
case TP_DEGAGEMENT:
|
|
if(Localisation_get().y_mm < 360){
|
|
etat_tourne_panneaux = TP_DEGAGEMENT_ACTIF;
|
|
}else{
|
|
commande_vitesse_stop();
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
case TP_DEGAGEMENT_ACTIF:
|
|
return Strategie_aller_a(Localisation_get().x_mm, 360, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
|
|
enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
enum etat_action_t etat_action;
|
|
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm, 400, pos_x, 400, pos_x, 140,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian, -30 * DEGRE_EN_RADIAN));
|
|
|
|
etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
|
|
return etat_action;
|
|
} |