Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
637d3213fa
RPiPico-Holonome2023/Strategie.c

646 lines
22 KiB
C
Raw Blame History

#include "hardware/gpio.h"
#include "i2c_annexe.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Score.h"
#include "Strategie_prise_cerises.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{
int priorite;
enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE} cible;
} 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();
enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
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,
DECISION_STRATEGIE,
}etat_strategie;
switch(etat_strategie){
case STRATEGIE_INIT:
if(couleur == COULEUR_BLEU){
Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
struct objectif_t objectif_4 = { .priorite = 4, .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 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 3, .etat = FAIT, .cible = CERISE_DROITE};
struct objectif_t objectif_4 = { .priorite = 4, .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];
etat_strategie = ALLER_CERISE_BAS;
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;
}
Trajet_config(250, 500);
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;
}
Trajet_config(250, 500);
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_sans_evitement(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(300, 250);
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
if(couleur == COULEUR_BLEU){
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
740, 3000 - 550,
510, 3000 - 1580,
180, 1500,
Localisation_get().angle_radian, angle_fin);
}else{
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
1225, 3000 - 540,
440, 3000 - 775,
225, 3000 - (1500 - 45),
Localisation_get().angle_radian, angle_fin);
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
Trajet_config(500, 500);
}
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_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->priorite > objectifs[m_objectif].priorite){
objectif_courant = &(objectifs[m_objectif]);
}
}
if(objectif_courant->etat == FAIT){
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 STRATEGIE_FIN:
i2c_annexe_desactive_propulseur();
commande_vitesse_stop();
break;
}
}
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(500, 500);
// 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-465, 2830,
-150. * DEGRE_EN_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,
-150. * DEGRE_EN_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;
}
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 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 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 = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_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(500, 500);
}
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;
}
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
float angle_avancement;
static enum {
PARCOURS_INIT,
PARCOURS_AVANCE,
} etat_parcourt=PARCOURS_INIT;
switch (etat_parcourt){
case PARCOURS_INIT:
Trajet_debut_trajectoire(trajectoire);
etat_parcourt = PARCOURS_AVANCE;
break;
case PARCOURS_AVANCE:
angle_avancement = Trajet_get_orientation_avance();
distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
Trajet_set_obstacle_mm(distance_obstacle);
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
etat_action = ACTION_TERMINEE;
etat_parcourt = PARCOURS_INIT;
}
break;
}
return etat_action;
}
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
static enum {
PARCOURS_INIT,
PARCOURS_AVANCE,
} etat_parcourt=PARCOURS_INIT;
switch (etat_parcourt){
case PARCOURS_INIT:
Trajet_debut_trajectoire(trajectoire);
Trajet_set_obstacle_mm(distance_pas_obstacle);
etat_parcourt = PARCOURS_AVANCE;
break;
case PARCOURS_AVANCE:
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_action = ACTION_TERMINEE;
etat_parcourt = PARCOURS_INIT;
}
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;
}
}
Reference in New Issue View Git Blame Copy Permalink