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

Travail en cours

Browse Source
This commit is contained in:
Samuel 2023-04-01 16:44:08 +02:00
parent a2e488b337
commit 6fa6b95789
4 changed files with 107 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
Strategie.c
View File

@ -69,19 +69,68 @@ void Homologation(uint32_t step_ms){
break;
case APPROCHE_PANIER_2:
if( depose_cerises(step_ms) == ACTION_TERMINEE){
etat_strategie = APPROCHE_CERISE_2;
}
break;
case APPROCHE_CERISE_2:
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
265,2830,
+120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
857, 3000 - 156,
+270. * DEGREE_EN_RADIAN, 300. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = ATTRAPPE_CERISE_2;
}
break;
case ATTRAPPE_CERISE_2:
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_3;
}
break;
case APPROCHE_PANIER_3:
if( depose_cerises(step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
}
break;
case STRATEGIE_FIN:
Moteur_Stop();
i2c_annexe_desactive_propulseur();
break;
}
}
enum etat_action_t depose_cerises(uint32_t step_ms){
static enum {
APPROCHE_ANGLE,
CALAGE_PANIER_1,
RECULE_PANIER,
LANCE_DANS_PANIER,
}etat_depose = APPROCHE_ANGLE;
enum etat_action_t etat_action = ACTION_EN_COURS;
struct trajectoire_t trajectoire;
switch(etat_depose){
case APPROCHE_ANGLE:
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
265, 2830,
Localisation_get().angle_radian, +120. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
etat_depose = CALAGE_PANIER_1;
}
break;
case CALAGE_PANIER_1:
if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
etat_strategie = RECULE_PANIER;
etat_depose = RECULE_PANIER;
}
break;
@ -92,23 +141,20 @@ void Homologation(uint32_t step_ms){
120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCE_DANS_PANIER;
etat_depose = LANCE_DANS_PANIER;
}
break;
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
etat_depose = APPROCHE_ANGLE;
etat_action = ACTION_TERMINEE;
}
break;
case STRATEGIE_FIN:
Moteur_Stop();
i2c_annexe_desactive_propulseur();
break;
}
return etat_action;
}
@ -212,6 +258,32 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
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);
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);

6
Strategie.h
View File

@ -35,12 +35,18 @@ extern enum etat_strategie_t{
RECULE_PANIER,
TOURNE_PANIER,
LANCE_DANS_PANIER,
APPROCHE_CERISE_2,
ATTRAPPE_CERISE_2,
APPROCHE_PANIER_3,
STRATEGIE_FIN,
}etat_strategie;
enum etat_action_t cerise_accostage(void);
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t depose_cerises(uint32_t step_ms);
void Homologation(uint32_t step_ms);
enum couleur_t lire_couleur(void);
uint attente_tirette(void);

19
Strategie_prise_cerises.c
View File

@ -13,6 +13,7 @@
// Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure.
#define MAX_LONGE_MM 240
#define MAX_ASPIRE_CERISE_MM 320
// Translation en mm/s pour aspirer les cerises
#define TRANSLATION_CERISE 70
@ -40,7 +41,8 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
TURBINE_DEMARRAGE,
TURBINE_DEMARRAGE_TEMPO,
ASPIRE_LONGE,
ASPIRE_LIBRE
ASPIRE_LIBRE,
ASPIRE_FIN
} etat_attrape=ATTRAPE_INIT;
switch(etat_attrape){
@ -68,7 +70,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
case TURBINE_DEMARRAGE_TEMPO:
if(tempo_ms < step_ms){
etat_attrape = ASPIRE_LONGE;
printf(">e:ASPIRE_LONGE|t\n");
}else{
tempo_ms -= step_ms;
}
@ -83,11 +84,23 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) ||
((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
etat_attrape = ASPIRE_LIBRE;
printf(">e:ASPIRE_LIBRE|t\n");
}
break;
case ASPIRE_LIBRE:
longer_direction_aspire = inverser_longe_direction(longer_direction);
if(longer_direction_aspire == LONGER_VERS_A){
commande_translation_longer_vers_A();
}else{
commande_translation_longer_vers_C();
}
if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_ASPIRE_CERISE_MM) )) ||
((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_ASPIRE_CERISE_MM))) ){
etat_attrape = ASPIRE_FIN;
}
break;
case ASPIRE_FIN:
commande_vitesse_stop();
i2c_annexe_desactive_turbine();
etat_action = ACTION_TERMINEE;

1
Test_strategie.c
View File

@ -106,6 +106,7 @@ int test_strategie(){
}
}
int test_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
printf("Homologation\n");