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

Homologation : Approche panier + correction bug "longe cerise"

Browse Source
This commit is contained in:
Samuel 2023-03-27 20:38:37 +02:00
parent 82e0f68d78
commit 2e38c678fd
2 changed files with 48 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
Strategie.c
View File

@ -13,6 +13,9 @@ void Homologation(uint32_t step_ms){
APPROCHE_CERISE_1_A,
APPROCHE_CERISE_1_B,
ATTRAPE_CERISE_1,
APPROCHE_PANIER_1_A,
APPROCHE_PANIER_1_B,
CALAGE_PANIER_1,
STRATEGIE_FIN
}etat_strategie=STRATEGIE_INIT;
@ -44,10 +47,29 @@ void Homologation(uint32_t step_ms){
case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
etat_strategie = APPROCHE_PANIER_1_A;
}
break;
case APPROCHE_PANIER_1_A:
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
465, 857,
465,2830,
+30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
Trajet_debut_trajectoire(trajectoire);
etat_strategie = APPROCHE_PANIER_1_B;
break;
case APPROCHE_PANIER_1_B:
etat_trajet = Trajet_avance(step_ms/1000.);
if(etat_trajet == TRAJET_TERMINE){
etat_strategie = ATTRAPE_CERISE_1;
}
break;
case STRATEGIE_FIN:
break;

55
Strategie_prise_cerises.c
View File

@ -46,7 +46,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
switch(etat_attrape){
case ATTRAPE_INIT:
etat_attrape=ATTRAPE_VERS_BORDURE;
printf("ATTRAPE_VERS_BORDURE\n");
break;
case ATTRAPE_VERS_BORDURE:
@ -54,7 +53,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
(longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
etat_attrape = TURBINE_DEMARRAGE;
printf("TURBINE_DEMARRAGE\n");
}
break;
@ -64,27 +62,28 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
commande_vitesse_stop();
tempo_ms = 2000;
etat_attrape = TURBINE_DEMARRAGE_TEMPO;
printf("TURBINE_DEMARRAGE_TEMPO\n");
break;
case TURBINE_DEMARRAGE_TEMPO:
if(tempo_ms < step_ms){
etat_attrape = ASPIRE_LONGE;
printf("ASPIRE_LONGE\n");
printf(">e:ASPIRE_LONGE|t\n");
}else{
tempo_ms -= step_ms;
}
tempo_ms -= step_ms;
break;
case ASPIRE_LONGE:
longer_direction_aspire = inverser_longe_direction(longer_direction);
cerise_longer_bord(longer_direction_aspire);
cerise_longer_bord(LONGER_VERS_A);
// La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
// Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition
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("ASPIRE_LIBRE\n");
printf(">e:ASPIRE_LIBRE|t\n");
}
break;
@ -104,43 +103,37 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
static enum {
LONGE_INIT,
LONGE_VERS_A,
LONGE_VERS_C,
LONGE_NORMAL,
LONGE_COLLE
} etat_longer_bord=LONGE_INIT;
switch (etat_longer_bord){
case LONGE_INIT:
etat_longer_bord=LONGE_NORMAL;
break;
case LONGE_NORMAL:
if(longer_direction==LONGER_VERS_A){
etat_longer_bord=LONGE_VERS_A;
commande_translation_longer_vers_A();
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
etat_longer_bord = LONGE_COLLE;
}
}else{
// longer_direction==LONGER_VERS_C
etat_longer_bord=LONGE_VERS_C;
}
break;
commande_translation_longer_vers_C();
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
etat_longer_bord = LONGE_COLLE;
}
case LONGE_VERS_A:
commande_translation_longer_vers_A();
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
etat_longer_bord = LONGE_COLLE;
printf("Longer colle\n");
}
break;
case LONGE_VERS_C:
commande_translation_longer_vers_C();
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
etat_longer_bord = LONGE_COLLE;
printf("Longer colle\n");
}
break;
case LONGE_COLLE:
if(cerise_accostage() == ACTION_TERMINEE){
etat_longer_bord = LONGE_INIT;
printf("Longer INIT\n");
etat_longer_bord = LONGE_NORMAL;
}
}
return ACTION_EN_COURS;