Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Gestion du retour à la maison fonctionnel

Browse Source
This commit is contained in:
Samuel 2023-05-19 23:40:07 +02:00
parent a0f6044799
commit 727197d9f4
4 changed files with 58 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Monitoring.c
View File

@ -5,6 +5,7 @@
#include "Trajet.h" #include "Trajet.h"
#include "Asser_Moteurs.h" #include "Asser_Moteurs.h"
#include "i2c_annexe.h" #include "i2c_annexe.h"
#include "Evitement.h"
#define LED_PIN_ROUGE 28 #define LED_PIN_ROUGE 28
@ -45,6 +46,7 @@ void Monitoring_display(){
printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.)); printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.));
printf(">DebugVar:%ld:%d\n", temps, debug_var); printf(">DebugVar:%ld:%d\n", temps, debug_var);
printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm()); printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">Etat_Evitement:%ld:%d\n", temps, Evitement_get_statu());
printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf); printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf);
struct position_t position = Localisation_get(); struct position_t position = Localisation_get();
printf(">pos_x:%ld:%f\n", temps, position.x_mm); printf(">pos_x:%ld:%f\n", temps, position.x_mm);

73
Strategie.c
View File

@ -52,8 +52,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
//const uint32_t temps_pre_fin_match = (97000 - 15000); const uint32_t temps_pre_fin_match = (97000 - 15000);
const uint32_t temps_pre_fin_match = (15000);
static bool pre_fin_match_active=false; static bool pre_fin_match_active=false;
float angle_fin; float angle_fin;
float recal_pos_x, recal_pos_y; float recal_pos_x, recal_pos_y;
@ -85,7 +84,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_strategie=STRATEGIE_FIN; etat_strategie=STRATEGIE_FIN;
} }
if(temps_ms > temps_pre_fin_match){ if(temps_ms > temps_pre_fin_match && pre_fin_match_active == false){
if(etat_strategie != LANCER_PANIER){ if(etat_strategie != LANCER_PANIER){
etat_strategie = RETOUR_MAISON; etat_strategie = RETOUR_MAISON;
} }
@ -98,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); 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_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_GAUCHE}; 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}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
objectifs[0]= objectif_1; objectifs[0]= objectif_1;
objectifs[1]= objectif_2; objectifs[1]= objectif_2;
@ -108,7 +107,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); 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_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_DROITE}; 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}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
objectifs[0]= objectif_1; objectifs[0]= objectif_1;
objectifs[1]= objectif_2; objectifs[1]= objectif_2;
@ -291,8 +290,22 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
} }
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: case STRATEGIE_FIN:
pre_fin_match_active=true;
i2c_annexe_desactive_propulseur(); i2c_annexe_desactive_propulseur();
commande_vitesse_stop(); commande_vitesse_stop();
i2c_annexe_couleur_balise(0xE0, 0x0FFF); i2c_annexe_couleur_balise(0xE0, 0x0FFF);
@ -459,6 +472,25 @@ int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
return 0; 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 /// @brief Renvoi 1 si le robot intersect une zone de dépose
/// @param couleur du robot /// @param couleur du robot
/// @return 1 si oui, 0 si non. /// @return 1 si oui, 0 si non.
@ -787,20 +819,23 @@ enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms)
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){ 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 = &objectifs_plats[0]; static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL;
enum etat_action_t etat_action; enum etat_action_t etat_action;
//Trajet_config(500,500); //750, 500 => Ne marche pas
if(objectif_plat_courant==NULL){
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1}; objectif_plat_courant = &objectifs_plats[0];
struct objectif_t objectif_2 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_2}; struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1};
struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_3}; struct objectif_t objectif_2 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_2};
struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4}; struct objectif_t objectif_3 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_3};
struct objectif_t objectif_5 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_5}; struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4};
objectifs_plats[0] = objectif_1; struct objectif_t objectif_5 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_5};
objectifs_plats[1] = objectif_2; objectifs_plats[0] = objectif_1;
objectifs_plats[2] = objectif_3; objectifs_plats[1] = objectif_2;
objectifs_plats[3] = objectif_4; objectifs_plats[2] = objectif_3;
objectifs_plats[4] = objectif_5; objectifs_plats[3] = objectif_4;
objectifs_plats[4] = objectif_5;
}
etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms); etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms);
switch(etat_action){ switch(etat_action){
@ -829,7 +864,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st
// Si notre objectif est FAIT, on prend le premier objectif "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. // 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++){ for(uint i=0; i < 5; i++){
if(objectif_plat_courant->etat == FAIT && objectifs_plats[i].etat == A_FAIRE){ if(objectif_plat_courant->etat == BLOQUE && objectifs_plats[i].etat == A_FAIRE){
objectif_plat_courant = &(objectifs_plats[i]); objectif_plat_courant = &(objectifs_plats[i]);
}else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){ }else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){
if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){ if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){
@ -838,7 +873,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st
} }
} }
return ACTION_EN_COURS;
} }

1
Strategie.h
View File

@ -57,6 +57,7 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement); enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
int Robot_est_dans_zone_depose(enum couleur_t couleur); int Robot_est_dans_zone_depose(enum couleur_t couleur);
int Robot_est_dans_zone_depose_panier(enum couleur_t couleur);
enum etat_action_t Strategie_preparation(); enum etat_action_t Strategie_preparation();
enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms);

2
Strategie_deplacement.c
View File

@ -27,7 +27,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
Trajet_set_obstacle_mm(distance_obstacle); Trajet_set_obstacle_mm(distance_obstacle);
} }
if(Evitement_get_statu() == ARRET_DEVANT_OBSTACLE){ if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
switch(evitement){ switch(evitement){
case SANS_EVITEMENT: case SANS_EVITEMENT:
//printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n"); //printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n");