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

Preparation Finales - Cerise du bas + augmentation des vitesses

Browse Source
This commit is contained in:
Samuel 2023-05-20 13:07:54 +02:00
parent 0e45582a6b
commit 4d29a0918a
4 changed files with 38 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
Strategie.c
View File

@ -53,7 +53,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 - 10000);
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;
@ -96,9 +96,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
case STRATEGIE_INIT: case STRATEGIE_INIT:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
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 = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS};
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;
@ -106,17 +106,16 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
objectifs[3]= objectif_4; objectifs[3]= objectif_4;
}else{ }else{
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 = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS};
, .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;
objectifs[2]= objectif_3; objectifs[2]= objectif_3;
objectifs[3]= objectif_4; objectifs[3]= objectif_4;
} }
objectif_courant = &objectifs[0]; objectif_courant = &objectifs[4];
Strategie_set_cerise_dans_robot(10); Strategie_set_cerise_dans_robot(10);
etat_strategie = LANCER_PANIER; etat_strategie = LANCER_PANIER;
break; break;
@ -125,18 +124,24 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
angle_fin = 30. * DEGRE_EN_RADIAN; angle_fin = 30. * DEGRE_EN_RADIAN;
point_x = 857; angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
650, Localisation_get().y_mm,
490, 0,
857, 0,
Localisation_get().angle_radian, angle_fin);
}else{ }else{
angle_fin = -150. * DEGRE_EN_RADIAN; angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 2000 - 857;
}
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Trajet_config(250, 250); 2000 - 650, Localisation_get().y_mm,
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156, 2000 - 490, 0,
2000 - 857, 0,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
}
Trajet_config(500,250);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_BAS; etat_strategie = ATTRAPER_CERISE_BAS;
} }
break; break;
@ -169,7 +174,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
} }
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 250); Trajet_config(500, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
@ -289,6 +294,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
case RETOUR_MAISON: case RETOUR_MAISON:
i2c_annexe_plie_bras(); i2c_annexe_plie_bras();
i2c_annexe_desactive_turbine();
if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){ 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 // 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)){ if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){
@ -358,7 +364,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
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 etat_action = ACTION_EN_COURS; enum etat_action_t etat_action = ACTION_EN_COURS;
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
Trajet_config(250, 250); Trajet_config(TRAJECT_CONFIG_STD);
// Definition des trajectoires // Definition des trajectoires
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
@ -378,6 +384,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
}else{ }else{
// Sinon, on a une courbe de bézier // Sinon, on a une courbe de bézier
Trajet_config(500,250);
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm, 485, Localisation_get().y_mm,
465, 857, 465, 857,
@ -401,6 +408,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
}else{ }else{
// Sinon, on a une courbe de bézier // Sinon, on a une courbe de bézier
Trajet_config(500,250);
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
2000-485, Localisation_get().y_mm, 2000-485, Localisation_get().y_mm,
2000-465, 857, 2000-465, 857,
@ -411,7 +419,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step
} }
// parcours du trajet // parcours du trajet
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == ACTION_TERMINEE){
etat_action = ACTION_TERMINEE; etat_action = ACTION_TERMINEE;
} }
return etat_action; return etat_action;

6
Strategie_deplacement.c
View File

@ -8,6 +8,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
enum etat_action_t etat_action = ACTION_EN_COURS; enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet; enum etat_trajet_t etat_trajet;
float angle_avancement; float angle_avancement;
static bool trajet_inverse = false;
static enum { static enum {
PARCOURS_INIT, PARCOURS_INIT,
@ -41,6 +42,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
return ACTION_ECHEC; return ACTION_ECHEC;
case RETOUR_SI_OBSTABLE: case RETOUR_SI_OBSTABLE:
trajet_inverse = !trajet_inverse;
Trajet_inverse(); Trajet_inverse();
break; break;
@ -53,7 +55,11 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
etat_trajet = Trajet_avance(step_ms/1000.); etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){ if(etat_trajet == TRAJET_TERMINE){
if(trajet_inverse){
etat_action = ACTION_ECHEC;
}else{
etat_action = ACTION_TERMINEE; etat_action = ACTION_TERMINEE;
}
etat_parcourt = PARCOURS_INIT; etat_parcourt = PARCOURS_INIT;
} }
break; break;

2
Strategie_pousse_gateau.c
View File

@ -48,7 +48,7 @@ enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms
} }
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 250); Trajet_config(500,250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);

2
Trajet.h
View File

@ -10,6 +10,8 @@ enum etat_trajet_t{
#define TRAJECT_CONFIG_AVANCE_DROIT 1000, 500 #define TRAJECT_CONFIG_AVANCE_DROIT 1000, 500
// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²) // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
// Vitesse et acceleration - standard (en mm et mm/s²)
#define TRAJECT_CONFIG_STD 500, 500
// Vitesse et acceleration pour une rotation (rad/s et rad/s²) // Vitesse et acceleration pour une rotation (rad/s et rad/s²)
#define TRAJECT_CONFIG_ROTATION_PURE 2, 2 #define TRAJECT_CONFIG_ROTATION_PURE 2, 2