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

Pousse sans servomoteur

Browse Source
This commit is contained in:
Samuel 2023-05-20 02:19:41 +02:00
parent cf1fc7183b
commit 41459008fa
4 changed files with 58 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -31,6 +31,7 @@ Servomoteur.c
Strategie.c Strategie.c
Strategie_deplacement.c Strategie_deplacement.c
Strategie_prise_cerises.c Strategie_prise_cerises.c
Strategie_pousse_gateau.c
Temps.c Temps.c
Test.c Test.c
Test_strategie.c Test_strategie.c

18
Strategie.c
View File

@ -97,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
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 = 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 = FAIT, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .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_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;
@ -107,8 +107,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
}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 = 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 = FAIT, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .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;
@ -206,10 +207,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_strategie = ATTRAPER_CERISE_GAUCHE; etat_strategie = ATTRAPER_CERISE_GAUCHE;
} }
} }
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
break; break;
case ATTRAPER_CERISE_GAUCHE: case ATTRAPER_CERISE_GAUCHE:
@ -231,10 +228,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
etat_strategie = ATTRAPER_CERISE_DROITE; etat_strategie = ATTRAPER_CERISE_DROITE;
} }
} }
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPER_CERISE_GAUCHE;
}
break; break;
case ATTRAPER_CERISE_DROITE: case ATTRAPER_CERISE_DROITE:
@ -315,9 +308,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
} }
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_proches(enum couleur_t couleur, uint32_t step_ms){
struct trajectoire_t trajectoire;
float angle_fin;
return Gateau_pousse_proche(couleur, step_ms); return Gateau_pousse_proche(couleur, step_ms);
/*if(couleur == COULEUR_BLEU){ /*if(couleur == COULEUR_BLEU){

1
Strategie.h
View File

@ -62,6 +62,7 @@ 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);
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);
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms);
extern float distance_obstacle; extern float distance_obstacle;

85
Strategie_pousse_gateau.c
View File

@ -1,5 +1,6 @@
#include "i2c_annexe.h" #include "i2c_annexe.h"
#include "Localisation.h" #include "Localisation.h"
#include "Monitoring.h"
#include "Strategie.h" #include "Strategie.h"
#include "Trajectoire.h" #include "Trajectoire.h"
#include "Trajet.h" #include "Trajet.h"
@ -7,57 +8,75 @@
enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
static enum { static enum {
GATEAU_PROCHE_PRE_POUSSE, ALLER_1,
GATEAU_PROCHE_POUSSE, ALLER_2,
GATEAU_PROCHE_RELACHE, ALLER_3
} etat_pousse_proche; } etat=ALLER_1;
float point_x, point_y;
float angle_fin;
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
float angle_fin, point_x, point_y;
switch(etat){
switch(etat_pousse_proche){ case ALLER_1:
case GATEAU_PROCHE_PRE_POUSSE:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
angle_fin = 90. * DEGRE_EN_RADIAN; angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 310; point_x = 225;
}else{ }else{
angle_fin = -135. * DEGRE_EN_RADIAN; angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 1655; point_x = 1775;
} }
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(100, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 360, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){ if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_pousse_proche = GATEAU_PROCHE_POUSSE; etat=ALLER_2;
i2c_annexe_deplie_bras();
} }
break; break;
case GATEAU_PROCHE_POUSSE: case ALLER_2:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
angle_fin = 90. * DEGRE_EN_RADIAN; angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 390; point_x = 225;
}else{ }else{
angle_fin = -135. * DEGRE_EN_RADIAN; angle_fin = 180. * DEGRE_EN_RADIAN;
point_x = 1585; point_x = 1775;
} }
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(250, 250);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1665, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1650,
Localisation_get().angle_radian, angle_fin); Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){ if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_pousse_proche = GATEAU_PROCHE_POUSSE; etat=ALLER_3;
i2c_annexe_plie_bras(); }
break;
case ALLER_3:
if(couleur == COULEUR_BLEU){
angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 225;
}else{
angle_fin = 30. * DEGRE_EN_RADIAN;
point_x = 1775;
}
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 - 1500,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat=ALLER_1;
return ACTION_TERMINEE;
} }
break; break;
case GATEAU_PROCHE_RELACHE:
} }
return ACTION_EN_COURS;
} }