93 lines
3.8 KiB
C
93 lines
3.8 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"
|
|
|
|
/// @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,
|
|
} etat_tourne_panneaux = TP_APPROCHE_PANNEAU_1;
|
|
|
|
|
|
switch (etat_tourne_panneaux)
|
|
{
|
|
case TP_APPROCHE_PANNEAU_1:
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
if(couleur == COULEUR_BLEU){
|
|
etat_action = Strategie_tourner_et_aller_a(
|
|
323, 300, (150. *DEGRE_EN_RADIAN),
|
|
EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
}else{
|
|
etat_action = Strategie_tourner_et_aller_a(
|
|
323, 300, (150. *DEGRE_EN_RADIAN),
|
|
EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
|
|
}
|
|
if (etat_action == ACTION_TERMINEE){
|
|
etat_tourne_panneaux = TP_TOURNE_PANNEAU_1;
|
|
}else if(etat_action == ACTION_ECHEC){
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
case TP_TOURNE_PANNEAU_1:
|
|
if(couleur == COULEUR_BLEU){
|
|
etat_action = Strategie_tourner_et_aller_a(
|
|
323, 200, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
|
|
}else{
|
|
etat_action = Strategie_tourner_et_aller_a(
|
|
323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
|
|
}
|
|
if (etat_action == ACTION_TERMINEE){
|
|
Score_ajout_panneau(1);
|
|
etat_tourne_panneaux = TP_TOURNE_PANNEAU_2;
|
|
}
|
|
break;
|
|
|
|
case TP_TOURNE_PANNEAU_2:
|
|
if(couleur == COULEUR_BLEU){
|
|
Trajectoire_bezier(&trajectoire, 323, 200, 405, 310, 548, 400, 548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
|
|
}else{
|
|
Trajectoire_bezier(&trajectoire, 3000-323, 200, 3000-405, 310, 3000-548, 400, 3000-548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
|
|
}
|
|
etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
|
|
if (etat_action == ACTION_TERMINEE){
|
|
Score_ajout_panneau(1);
|
|
etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
|
|
}else if(etat_action == ACTION_ECHEC){
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
case TP_TOURNE_PANNEAU_3:
|
|
if(couleur == COULEUR_BLEU){
|
|
Trajectoire_bezier(&trajectoire, 548, 200, 630, 310, 773, 400, 773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
|
|
}else{
|
|
Trajectoire_bezier(&trajectoire, 3000-548, 200, 3000-630, 310, 3000-773, 400, 3000-773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
|
|
}
|
|
etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
|
|
if (etat_action == ACTION_TERMINEE){
|
|
Score_ajout_panneau(1);
|
|
//etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
|
|
return ACTION_TERMINEE;
|
|
}else if(etat_action == ACTION_ECHEC){
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
} |