929 lines
33 KiB
C
929 lines
33 KiB
C
#include "hardware/gpio.h"
|
|
#include "i2c_annexe.h"
|
|
#include "Asser_Position.h"
|
|
#include "Commande_vitesse.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "Localisation.h"
|
|
#include "Monitoring.h"
|
|
#include "Moteurs.h"
|
|
#include "Score.h"
|
|
#include "Strategie_prise_cerises.h"
|
|
#include "Strategie_pousse_gateau.h"
|
|
#include "Strategie.h"
|
|
#include "Trajet.h"
|
|
#include "math.h"
|
|
|
|
#define SEUIL_RECAL_DIST_MM 75
|
|
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN)
|
|
#define DISTANCE_OBSTACLE_CM 50
|
|
#define DISTANCE_PAS_OBSTACLE_MM 2000
|
|
|
|
#define NB_OBJECTIFS 4
|
|
|
|
struct objectif_t objectifs[NB_OBJECTIFS];
|
|
struct objectif_t * objectif_courant;
|
|
|
|
uint32_t Score_cerises_dans_robot=0;
|
|
|
|
void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){
|
|
Score_cerises_dans_robot = nb_cerises;
|
|
}
|
|
|
|
uint32_t Strategie_get_cerise_dans_robot(void){
|
|
return Score_cerises_dans_robot;
|
|
}
|
|
|
|
|
|
|
|
// TODO: Peut-être à remetttre en variable locale après
|
|
float distance_obstacle;
|
|
|
|
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
|
|
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
|
|
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
|
|
|
|
|
|
int Robot_est_dans_quart_haut_gauche();
|
|
int Robot_est_dans_quart_haut_droit();
|
|
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur);
|
|
int Robot_est_dans_zone_cerise_gauche();
|
|
int Robot_est_dans_zone_cerise_droite();
|
|
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
|
|
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);
|
|
|
|
|
|
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
|
|
const uint32_t temps_pre_fin_match = (97000 - 15000);
|
|
static bool pre_fin_match_active=false;
|
|
float angle_fin;
|
|
float recal_pos_x, recal_pos_y;
|
|
float point_x, point_y;
|
|
enum longer_direction_t longer_direction;
|
|
enum etat_action_t etat_action;
|
|
enum etat_trajet_t etat_trajet;
|
|
|
|
struct trajectoire_t trajectoire;
|
|
|
|
static enum etat_strategie_t {
|
|
STRATEGIE_INIT,
|
|
ALLER_CERISE_BAS,
|
|
ATTRAPER_CERISE_BAS,
|
|
ALLER_CERISE_HAUT,
|
|
ATTRAPER_CERISE_HAUT,
|
|
ALLER_CERISE_GAUCHE,
|
|
ATTRAPER_CERISE_GAUCHE,
|
|
ALLER_CERISE_DROITE,
|
|
ATTRAPER_CERISE_DROITE,
|
|
ALLER_PANIER,
|
|
LANCER_PANIER,
|
|
RETOUR_MAISON,
|
|
DECISION_STRATEGIE,
|
|
STRATEGIE_FIN
|
|
}etat_strategie;
|
|
|
|
if(Monitoring_get_erreur_critique()){
|
|
etat_strategie=STRATEGIE_FIN;
|
|
}
|
|
|
|
if(temps_ms > temps_pre_fin_match && pre_fin_match_active == false){
|
|
if(etat_strategie != LANCER_PANIER){
|
|
etat_strategie = RETOUR_MAISON;
|
|
}
|
|
pre_fin_match_active=true;
|
|
}
|
|
|
|
switch(etat_strategie){
|
|
case STRATEGIE_INIT:
|
|
if(couleur == COULEUR_BLEU){
|
|
Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
|
|
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
|
|
struct objectif_t objectif_2 = { .priorite = 1, .etat = FAIT, .cible = CERISE_HAUT};
|
|
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
|
|
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
|
|
objectifs[0]= objectif_1;
|
|
objectifs[1]= objectif_2;
|
|
objectifs[2]= objectif_3;
|
|
objectifs[3]= objectif_4;
|
|
}else{
|
|
Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
|
|
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
|
|
struct objectif_t objectif_2 = { .priorite = 1, .etat = FAIT, .cible = CERISE_HAUT};
|
|
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE
|
|
, .cible = CERISE_DROITE};
|
|
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
|
|
objectifs[0]= objectif_1;
|
|
objectifs[1]= objectif_2;
|
|
objectifs[2]= objectif_3;
|
|
objectifs[3]= objectif_4;
|
|
}
|
|
objectif_courant = &objectifs[0];
|
|
Strategie_set_cerise_dans_robot(10);
|
|
etat_strategie = LANCER_PANIER;
|
|
break;
|
|
|
|
case ALLER_CERISE_BAS:
|
|
|
|
if(couleur == COULEUR_BLEU){
|
|
angle_fin = 30. * DEGRE_EN_RADIAN;
|
|
point_x = 857;
|
|
}else{
|
|
angle_fin = -150. * DEGRE_EN_RADIAN;
|
|
point_x = 2000 - 857;
|
|
}
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
|
|
|
Trajet_config(250, 250);
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
|
|
etat_strategie = ATTRAPER_CERISE_BAS;
|
|
}
|
|
break;
|
|
|
|
case ATTRAPER_CERISE_BAS:
|
|
recal_pos_y = RAYON_ROBOT;
|
|
if(couleur == COULEUR_BLEU){
|
|
longer_direction = LONGER_VERS_C;
|
|
recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
|
|
}else{
|
|
longer_direction = LONGER_VERS_A;
|
|
recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
|
|
}
|
|
|
|
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
|
|
if(etat_action == ACTION_TERMINEE){
|
|
Strategie_set_cerise_dans_robot(6);
|
|
etat_strategie = ALLER_PANIER;
|
|
}
|
|
break;
|
|
|
|
case ALLER_CERISE_HAUT:
|
|
|
|
if(couleur == COULEUR_BLEU){
|
|
angle_fin = 30. * DEGRE_EN_RADIAN;
|
|
point_x = 857;
|
|
}else{
|
|
angle_fin = -150. * DEGRE_EN_RADIAN;
|
|
point_x = 2000 - 857;
|
|
}
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
|
|
|
Trajet_config(250, 250);
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
|
|
etat_strategie = ATTRAPER_CERISE_HAUT;
|
|
}
|
|
break;
|
|
|
|
case ATTRAPER_CERISE_HAUT:
|
|
recal_pos_y = 3000 - RAYON_ROBOT;
|
|
if(couleur == COULEUR_BLEU){
|
|
longer_direction = LONGER_VERS_A;
|
|
recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
|
|
}else{
|
|
longer_direction = LONGER_VERS_C;
|
|
recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
|
|
}
|
|
|
|
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
|
|
if(etat_action == ACTION_TERMINEE){
|
|
Strategie_set_cerise_dans_robot(6);
|
|
etat_strategie = ALLER_PANIER;
|
|
}
|
|
break;
|
|
|
|
case ALLER_CERISE_GAUCHE:
|
|
Trajet_config(250, 250);
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
|
if(couleur == COULEUR_BLEU){
|
|
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
|
|
etat_strategie = ATTRAPER_CERISE_GAUCHE;
|
|
}
|
|
}else{
|
|
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
|
|
etat_strategie = ATTRAPER_CERISE_GAUCHE;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case ATTRAPER_CERISE_GAUCHE:
|
|
if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){
|
|
Strategie_set_cerise_dans_robot(10);
|
|
etat_strategie = ALLER_PANIER;
|
|
}
|
|
break;
|
|
|
|
case ALLER_CERISE_DROITE:
|
|
Trajet_config(250, 250);
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
|
if(couleur == COULEUR_BLEU){
|
|
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
|
|
etat_strategie = ATTRAPER_CERISE_DROITE;
|
|
}
|
|
}else{
|
|
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
|
|
etat_strategie = ATTRAPER_CERISE_DROITE;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case ATTRAPER_CERISE_DROITE:
|
|
if(cerises_attraper_cerises_droite(step_ms) == ACTION_TERMINEE){
|
|
Strategie_set_cerise_dans_robot(10);
|
|
etat_strategie = ALLER_PANIER;
|
|
}
|
|
break;
|
|
|
|
case ALLER_PANIER:
|
|
if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){
|
|
etat_strategie = LANCER_PANIER;
|
|
}
|
|
break;
|
|
|
|
case LANCER_PANIER:
|
|
if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){
|
|
Score_ajout_cerise(Strategie_get_cerise_dans_robot());
|
|
Strategie_set_cerise_dans_robot(0);
|
|
objectif_courant->etat = FAIT;
|
|
etat_strategie = DECISION_STRATEGIE;
|
|
}
|
|
break;
|
|
|
|
case DECISION_STRATEGIE:
|
|
// Obtenir l'objectif suivant
|
|
for(uint m_objectif=0; m_objectif < NB_OBJECTIFS; m_objectif++){
|
|
if(objectif_courant->etat == FAIT && objectifs[m_objectif].etat == A_FAIRE){
|
|
objectif_courant = &(objectifs[m_objectif]);
|
|
}else if(objectif_courant->etat == A_FAIRE && objectifs[m_objectif].etat == A_FAIRE){
|
|
if(objectif_courant->priorite > objectifs[m_objectif].priorite){
|
|
objectif_courant = &(objectifs[m_objectif]);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(objectif_courant->etat == FAIT || pre_fin_match_active){
|
|
etat_strategie = STRATEGIE_FIN;
|
|
}else{
|
|
switch(objectif_courant->cible){
|
|
case CERISE_BAS:
|
|
etat_strategie = ALLER_CERISE_BAS;
|
|
break;
|
|
case CERISE_HAUT:
|
|
etat_strategie = ALLER_CERISE_HAUT;
|
|
break;
|
|
case CERISE_GAUCHE:
|
|
etat_strategie = ALLER_CERISE_GAUCHE;
|
|
break;
|
|
case CERISE_DROITE:
|
|
etat_strategie = ALLER_CERISE_DROITE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
break;
|
|
|
|
case RETOUR_MAISON:
|
|
if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){
|
|
// Si le robot est dans la zone du panier, jeter les cerises s'il en a
|
|
if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){
|
|
//etat_strategie=LANCER_PANIER; // Il faut orienter le robot face au panier
|
|
etat_strategie=STRATEGIE_FIN;
|
|
}else{
|
|
etat_strategie=STRATEGIE_FIN;
|
|
}
|
|
}
|
|
break;
|
|
|
|
|
|
case STRATEGIE_FIN:
|
|
pre_fin_match_active=true;
|
|
i2c_annexe_desactive_propulseur();
|
|
commande_vitesse_stop();
|
|
i2c_annexe_couleur_balise(0xE0, 0x0FFF);
|
|
break;
|
|
}
|
|
}
|
|
|
|
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){
|
|
return Gateau_pousse_proche(couleur, step_ms);
|
|
|
|
/*if(couleur == COULEUR_BLEU){
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
|
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
740, 3000 - 550,
|
|
510, 3000 - 1580,
|
|
500, 1400,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
}else{
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
|
|
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
2000 - 740, 3000 - 550,
|
|
2000 - 510, 3000 - 1580,
|
|
2000 - 500, 1400,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
}
|
|
return parcourt_trajet_simple(trajectoire, step_ms);*/
|
|
}
|
|
|
|
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
float angle_fin;
|
|
if(couleur == COULEUR_BLEU){
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
|
|
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
785, 3000 - 550,
|
|
1550, 3000 - 785,
|
|
1775, 1500,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
}else{
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
|
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
2000 - 785, 3000 - 550,
|
|
2000 - 1550, 3000 - 785,
|
|
2000 - 1775, 1500,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
}
|
|
return parcourt_trajet_simple(trajectoire, step_ms);
|
|
}
|
|
|
|
|
|
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
|
|
enum etat_action_t etat_action = ACTION_EN_COURS;
|
|
struct trajectoire_t trajectoire;
|
|
Trajet_config(250, 250);
|
|
|
|
// Definition des trajectoires
|
|
if(couleur == COULEUR_BLEU){
|
|
// Si le robot est déjà dans la zone cerise haut
|
|
// On va en ligne droite
|
|
if(Robot_est_dans_zone_cerise_haut(couleur)){
|
|
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
180,2800,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
|
|
}else if (Robot_est_dans_zone_cerise_gauche()){
|
|
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm + 330, Localisation_get().y_mm - 100,
|
|
745, 3000 - 475,
|
|
180, 3000 - 200,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
|
|
}else{
|
|
// Sinon, on a une courbe de bézier
|
|
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
485, Localisation_get().y_mm,
|
|
465, 857,
|
|
465,2830,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
|
|
}
|
|
}else{ // COULEUR_VERT
|
|
// Si le robot est déjà dans le quart haut droit,
|
|
// On va en ligne droite
|
|
if(Robot_est_dans_quart_haut_droit()){
|
|
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
2000-180, 2800,
|
|
Localisation_get().angle_radian ,Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
|
|
}else if (Robot_est_dans_zone_cerise_droite()){
|
|
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm + 330, Localisation_get().y_mm - 100,
|
|
2000 - 745, 3000 - 475,
|
|
2000 - 180, 3000 - 200,
|
|
Localisation_get().angle_radian,
|
|
Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
|
|
}else{
|
|
// Sinon, on a une courbe de bézier
|
|
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
2000-485, Localisation_get().y_mm,
|
|
2000-465, 857,
|
|
2000-465, 2830,
|
|
Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
|
|
}
|
|
|
|
}
|
|
|
|
// parcours du trajet
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
|
etat_action = ACTION_TERMINEE;
|
|
}
|
|
return etat_action;
|
|
|
|
}
|
|
|
|
/// Fonction de localisation approximatives pour la stratégie.
|
|
|
|
/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
|
|
/// @return 1 si oui, 0 si non.
|
|
int Robot_est_dans_quart_haut_gauche(){
|
|
if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain
|
|
/// @return 1 si oui, 0 si non.
|
|
int Robot_est_dans_quart_haut_droit(){
|
|
if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int Robot_est_dans_zone_cerise_gauche(){
|
|
if(Localisation_get().x_mm < 630 &&
|
|
Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
|
|
}
|
|
|
|
int Robot_est_dans_zone_cerise_droite(){
|
|
if(Localisation_get().x_mm > 2000 - 630 &&
|
|
Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
|
|
}
|
|
|
|
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
|
|
if(couleur == COULEUR_BLEU){
|
|
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){
|
|
return 1;
|
|
}
|
|
}else{
|
|
if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm > 1000){
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
int Robot_est_dans_zone_depose_panier(enum couleur_t couleur){
|
|
float x_mm, y_mm;
|
|
x_mm = Localisation_get().x_mm;
|
|
y_mm = Localisation_get().y_mm;
|
|
if(couleur == COULEUR_BLEU){
|
|
// Zone 1
|
|
if(x_mm < 550 && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
}else{
|
|
// Zone 1
|
|
if(x_mm > 2000 - 550 && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Renvoi 1 si le robot intersect une zone de dépose
|
|
/// @param couleur du robot
|
|
/// @return 1 si oui, 0 si non.
|
|
int Robot_est_dans_zone_depose(enum couleur_t couleur){
|
|
float x_mm, y_mm;
|
|
x_mm = Localisation_get().x_mm;
|
|
y_mm = Localisation_get().y_mm;
|
|
if(couleur == COULEUR_BLEU){
|
|
// Zone 1
|
|
if(x_mm < 550 && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
// Zone 2
|
|
if(x_mm < 550 && y_mm > 800 && y_mm < 1450){
|
|
return 1;
|
|
}
|
|
// Zone 3
|
|
if(x_mm > 400 && x_mm < 1050 && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 4
|
|
if(x_mm > 1450 && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 5
|
|
if(x_mm > 1450 && y_mm > 1550 && y_mm < 2200){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
|
|
}else{
|
|
// VERT
|
|
// Zone 1
|
|
if(x_mm > (2000 - 550) && y_mm > 2450){
|
|
return 1;
|
|
}
|
|
// Zone 2
|
|
if(x_mm > (2000 - 550) && y_mm > 800 && y_mm < 1450){
|
|
return 1;
|
|
}
|
|
// Zone 3
|
|
if(x_mm < (2000 - 400) && x_mm > 2000 - 1050 && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 4
|
|
if(x_mm < (2000 - 1450) && y_mm < 550){
|
|
return 1;
|
|
}
|
|
// Zone 5
|
|
if(x_mm < (2000 - 1450) && y_mm > 1550 && y_mm < 2200){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
|
|
static enum {
|
|
CALAGE_PANIER_1,
|
|
RECULE_PANIER,
|
|
LANCE_DANS_PANIER,
|
|
}etat_lance_balles_dans_panier;
|
|
float recal_pos_x, recal_pos_y;
|
|
float angle_depart, angle_arrivee;
|
|
enum longer_direction_t longer_direction;
|
|
float point_x, point_y;
|
|
|
|
enum etat_action_t etat_action = ACTION_EN_COURS;
|
|
|
|
struct trajectoire_t trajectoire;
|
|
|
|
switch(etat_lance_balles_dans_panier){
|
|
case CALAGE_PANIER_1:
|
|
if(couleur == COULEUR_BLEU){
|
|
recal_pos_x = RAYON_ROBOT;
|
|
longer_direction = LONGER_VERS_A;
|
|
}else{
|
|
recal_pos_x = 2000- RAYON_ROBOT;
|
|
longer_direction = LONGER_VERS_C;
|
|
}
|
|
|
|
if(calage_angle(longer_direction, recal_pos_x, 3000 - (PETIT_RAYON_ROBOT_MM), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
|
|
etat_lance_balles_dans_panier = RECULE_PANIER;
|
|
}
|
|
break;
|
|
|
|
case RECULE_PANIER:
|
|
Trajet_config(120, 250);
|
|
if(couleur == COULEUR_BLEU){
|
|
point_x = 180;
|
|
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
|
|
}else{
|
|
point_x = 1735;
|
|
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
|
|
}
|
|
angle_depart = Localisation_get().angle_radian;
|
|
angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN);
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
point_x, point_y,
|
|
angle_depart, angle_arrivee);
|
|
|
|
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
|
|
etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
|
|
Trajet_config(250, 250);
|
|
}
|
|
break;
|
|
|
|
case LANCE_DANS_PANIER:
|
|
Asser_Position_maintien();
|
|
if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){
|
|
etat_action = ACTION_TERMINEE;
|
|
etat_lance_balles_dans_panier = CALAGE_PANIER_1;
|
|
}
|
|
break;
|
|
}
|
|
return etat_action;
|
|
|
|
}
|
|
|
|
/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
|
|
/// @param step_ms : pas de temps.
|
|
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
|
|
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){
|
|
enum etat_action_t etat_action = ACTION_EN_COURS;
|
|
static uint32_t tempo_ms;
|
|
static uint32_t nb_iteration;
|
|
|
|
static enum{
|
|
LANCE_PROPULSEUR_ON,
|
|
LANCE_TEMPO_PROP_ON,
|
|
LANCE_PORTE_OUVERTE,
|
|
LANCE_OUVERTURE_INITIALE,
|
|
} etat_lance_balle = LANCE_PROPULSEUR_ON;
|
|
|
|
switch(etat_lance_balle){
|
|
case LANCE_PROPULSEUR_ON:
|
|
i2c_annexe_active_propulseur();
|
|
tempo_ms = 1000;
|
|
nb_iteration=0;
|
|
etat_lance_balle = LANCE_OUVERTURE_INITIALE;
|
|
break;
|
|
|
|
case LANCE_OUVERTURE_INITIALE:
|
|
if(temporisation_terminee(&tempo_ms, step_ms)){
|
|
i2c_annexe_ouvre_porte();
|
|
etat_lance_balle = LANCE_PORTE_OUVERTE;
|
|
tempo_ms = 250;
|
|
}
|
|
break;
|
|
|
|
case LANCE_TEMPO_PROP_ON:
|
|
if(temporisation_terminee(&tempo_ms, step_ms)){
|
|
i2c_annexe_ouvre_porte();
|
|
nb_iteration++;
|
|
if(nb_iteration > nb_cerises){
|
|
nb_iteration=0;
|
|
etat_action = ACTION_TERMINEE;
|
|
etat_lance_balle = LANCE_PROPULSEUR_ON;
|
|
i2c_annexe_desactive_propulseur();
|
|
}else{
|
|
etat_lance_balle = LANCE_PORTE_OUVERTE;
|
|
tempo_ms = 150;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case LANCE_PORTE_OUVERTE:
|
|
if(temporisation_terminee(&tempo_ms, step_ms)){
|
|
i2c_annexe_mi_ferme_porte();
|
|
etat_lance_balle = LANCE_TEMPO_PROP_ON;
|
|
tempo_ms = 500;
|
|
}
|
|
break;
|
|
|
|
}
|
|
return etat_action;
|
|
}
|
|
|
|
/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
|
|
/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites
|
|
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){
|
|
enum etat_action_t etat_action = ACTION_EN_COURS;
|
|
struct position_t position;
|
|
struct trajectoire_t trajectoire;
|
|
static float y_pos_ref;
|
|
|
|
static enum {
|
|
CONTACT_AVANT,
|
|
RECULE_BORDURE,
|
|
CONTACT_LATERAL,
|
|
RECALE_X
|
|
}etat_calage_angle;
|
|
|
|
switch(etat_calage_angle){
|
|
case CONTACT_AVANT:
|
|
if(cerise_accostage() == ACTION_TERMINEE){
|
|
position = Localisation_get();
|
|
//if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
|
|
Localisation_set_y(y_mm);
|
|
//}
|
|
//if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
|
|
Localisation_set_angle(angle_radian);
|
|
//}
|
|
y_pos_ref = Localisation_get().y_mm;
|
|
etat_calage_angle = RECULE_BORDURE;
|
|
}
|
|
break;
|
|
|
|
case RECULE_BORDURE:
|
|
commande_translation_recule_vers_trompe();
|
|
position = Localisation_get();
|
|
if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){
|
|
etat_calage_angle = CONTACT_LATERAL;
|
|
}
|
|
break;
|
|
|
|
case CONTACT_LATERAL:
|
|
if(longer_direction == LONGER_VERS_A){
|
|
commande_translation_avance_vers_A();
|
|
if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){
|
|
etat_calage_angle = RECALE_X;
|
|
}
|
|
}else{
|
|
commande_translation_avance_vers_C();
|
|
if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){
|
|
etat_calage_angle = RECALE_X;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case RECALE_X:
|
|
etat_action = ACTION_TERMINEE;
|
|
commande_vitesse_stop();
|
|
position = Localisation_get();
|
|
//if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
|
|
Localisation_set_x(x_mm);
|
|
//}
|
|
etat_calage_angle = CONTACT_AVANT;
|
|
break;
|
|
|
|
}
|
|
|
|
return etat_action;
|
|
}
|
|
|
|
/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
|
|
uint attente_tirette(void){
|
|
return !gpio_get(TIRETTE);
|
|
}
|
|
|
|
/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
|
|
enum couleur_t lire_couleur(void){
|
|
if (gpio_get(COULEUR))
|
|
return COULEUR_VERT;
|
|
return COULEUR_BLEU;
|
|
|
|
}
|
|
|
|
/// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée
|
|
/// @param tempo_ms
|
|
/// @param step_ms
|
|
/// @return 1 si la temporisation est écoulée, 0 sinon.
|
|
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
|
|
if(*tempo_ms < step_ms){
|
|
return 1;
|
|
}else{
|
|
*tempo_ms -= step_ms;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/// @brief Gère le chargement des balles au début du match
|
|
enum etat_action_t Strategie_preparation(){
|
|
|
|
static enum{
|
|
PREP_INIT_V,
|
|
PREP_INIT_B,
|
|
PREP_ASPIRE,
|
|
PREP_ARRET_ASPIRATION,
|
|
PREP_TEMPO,
|
|
} etat_prep=PREP_INIT_V;
|
|
|
|
switch(etat_prep){
|
|
case PREP_INIT_V:
|
|
if(lire_couleur()== COULEUR_VERT){
|
|
etat_prep = PREP_INIT_B;
|
|
}
|
|
break;
|
|
case PREP_INIT_B:
|
|
if(lire_couleur()== COULEUR_BLEU){
|
|
etat_prep = PREP_ASPIRE;
|
|
}
|
|
break;
|
|
|
|
case PREP_ASPIRE:
|
|
if(lire_couleur()== COULEUR_VERT){
|
|
i2c_annexe_ferme_porte();
|
|
i2c_annexe_active_turbine();
|
|
etat_prep=PREP_ARRET_ASPIRATION;
|
|
}
|
|
break;
|
|
|
|
case PREP_ARRET_ASPIRATION:
|
|
if(lire_couleur()== COULEUR_BLEU){
|
|
i2c_annexe_desactive_turbine();
|
|
etat_prep=PREP_TEMPO;
|
|
}
|
|
break;
|
|
|
|
case PREP_TEMPO:
|
|
return ACTION_TERMINEE;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
}
|
|
|
|
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms){
|
|
struct trajectoire_t trajectoire;
|
|
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
pos_x, pos_y,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
|
|
return Strategie_parcourir_trajet(trajectoire, step_ms, ARRET_DEVANT_OBSTACLE);
|
|
}
|
|
|
|
|
|
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){
|
|
static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL;
|
|
enum etat_action_t etat_action;
|
|
//Trajet_config(500,500); //750, 500 => Ne marche pas
|
|
|
|
if(objectif_plat_courant==NULL){
|
|
objectif_plat_courant = &objectifs_plats[0];
|
|
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1};
|
|
struct objectif_t objectif_2 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_2};
|
|
struct objectif_t objectif_3 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_3};
|
|
struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4};
|
|
struct objectif_t objectif_5 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_5};
|
|
objectifs_plats[0] = objectif_1;
|
|
objectifs_plats[1] = objectif_2;
|
|
objectifs_plats[2] = objectif_3;
|
|
objectifs_plats[3] = objectif_4;
|
|
objectifs_plats[4] = objectif_5;
|
|
}
|
|
|
|
etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms);
|
|
switch(etat_action){
|
|
case ACTION_TERMINEE:
|
|
return ACTION_TERMINEE;
|
|
case ACTION_EN_COURS:
|
|
return ACTION_EN_COURS;
|
|
case ACTION_ECHEC:
|
|
// 1. Marquer comme bloqué l'objectif en cours
|
|
objectif_plat_courant->etat = BLOQUE;
|
|
// 2. Si tous les objectifs sont bloqués, les marquer tous comme A_FAIRE
|
|
uint8_t non_bloque = 0;
|
|
for(uint i=0 ; i<5 ; i++){
|
|
if(objectifs_plats[i].etat == A_FAIRE){
|
|
non_bloque=1;
|
|
}
|
|
}
|
|
if(!non_bloque){
|
|
for(uint i=0 ; i<5 ; i++){
|
|
if (objectifs_plats[i].etat == BLOQUE){
|
|
objectifs_plats[i].etat = A_FAIRE;
|
|
}
|
|
}
|
|
}
|
|
// 3. Trouver le prochain objectif (priorité la plus basse + etat A_FAIRE)
|
|
// Si notre objectif est FAIT, on prend le premier objectif "A_FAIRE"
|
|
// Si notre objectif est A_FAIRE, on prend le nouvel objectif que si sa priorité est plus basse.
|
|
for(uint i=0; i < 5; i++){
|
|
if(objectif_plat_courant->etat == BLOQUE && objectifs_plats[i].etat == A_FAIRE){
|
|
objectif_plat_courant = &(objectifs_plats[i]);
|
|
}else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){
|
|
if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){
|
|
objectif_plat_courant = &(objectifs_plats[i]);
|
|
}
|
|
|
|
}
|
|
}
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms){
|
|
float pos_x;
|
|
float pos_y;
|
|
|
|
switch (objectif_plat_courant->cible){
|
|
case ZONE_1:
|
|
pos_y = 2775;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 250;
|
|
}else{
|
|
pos_x = 2000 - 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, step_ms);
|
|
|
|
case ZONE_2:
|
|
pos_y = 1125;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 250;
|
|
}else{
|
|
pos_x = 2000 - 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, step_ms);
|
|
|
|
case ZONE_3:
|
|
pos_y = 250;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 725;
|
|
}else{
|
|
pos_x = 2000 - 725;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, step_ms);
|
|
|
|
case ZONE_4:
|
|
pos_y = 250;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 2000 - 250;
|
|
}else{
|
|
pos_x = 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, step_ms);
|
|
|
|
case ZONE_5:
|
|
pos_y = 1850;
|
|
if (couleur == COULEUR_BLEU){
|
|
pos_x = 2000 - 250;
|
|
}else{
|
|
pos_x = 250;
|
|
}
|
|
return Strategie_aller_a(pos_x, pos_y, step_ms);
|
|
}
|
|
}
|
|
|