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_A,
APPROCHE_CERISE_1_B, APPROCHE_CERISE_1_B,
ATTRAPE_CERISE_1, ATTRAPE_CERISE_1,
APPROCHE_PANIER_1_A,
APPROCHE_PANIER_1_B,
CALAGE_PANIER_1,
STRATEGIE_FIN STRATEGIE_FIN
}etat_strategie=STRATEGIE_INIT; }etat_strategie=STRATEGIE_INIT;
@ -44,10 +47,29 @@ void Homologation(uint32_t step_ms){
case ATTRAPE_CERISE_1: case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms); etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
if(etat_action == ACTION_TERMINEE){ if(etat_action == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN; etat_strategie = APPROCHE_PANIER_1_A;
} }
break; 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: case STRATEGIE_FIN:
break; break;

57
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){ switch(etat_attrape){
case ATTRAPE_INIT: case ATTRAPE_INIT:
etat_attrape=ATTRAPE_VERS_BORDURE; etat_attrape=ATTRAPE_VERS_BORDURE;
printf("ATTRAPE_VERS_BORDURE\n");
break; break;
case ATTRAPE_VERS_BORDURE: 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) || 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) ){ (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
etat_attrape = TURBINE_DEMARRAGE; etat_attrape = TURBINE_DEMARRAGE;
printf("TURBINE_DEMARRAGE\n");
} }
break; break;
@ -64,27 +62,28 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
commande_vitesse_stop(); commande_vitesse_stop();
tempo_ms = 2000; tempo_ms = 2000;
etat_attrape = TURBINE_DEMARRAGE_TEMPO; etat_attrape = TURBINE_DEMARRAGE_TEMPO;
printf("TURBINE_DEMARRAGE_TEMPO\n");
break; break;
case TURBINE_DEMARRAGE_TEMPO: case TURBINE_DEMARRAGE_TEMPO:
if(tempo_ms < step_ms){ if(tempo_ms < step_ms){
etat_attrape = ASPIRE_LONGE; etat_attrape = ASPIRE_LONGE;
printf("ASPIRE_LONGE\n"); printf(">e:ASPIRE_LONGE|t\n");
}else{
tempo_ms -= step_ms;
} }
tempo_ms -= step_ms;
break; break;
case ASPIRE_LONGE: case ASPIRE_LONGE:
longer_direction_aspire = inverser_longe_direction(longer_direction); 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 // 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 // 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 // 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) )) || 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))) ){ ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
etat_attrape = ASPIRE_LIBRE; etat_attrape = ASPIRE_LIBRE;
printf("ASPIRE_LIBRE\n"); printf(">e:ASPIRE_LIBRE|t\n");
} }
break; break;
@ -104,43 +103,37 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
static enum { static enum {
LONGE_INIT, LONGE_INIT,
LONGE_VERS_A, LONGE_NORMAL,
LONGE_VERS_C,
LONGE_COLLE LONGE_COLLE
} etat_longer_bord=LONGE_INIT; } etat_longer_bord=LONGE_INIT;
switch (etat_longer_bord){ switch (etat_longer_bord){
case LONGE_INIT: case LONGE_INIT:
if(longer_direction==LONGER_VERS_A){ etat_longer_bord=LONGE_NORMAL;
etat_longer_bord=LONGE_VERS_A;
}else{
// longer_direction==LONGER_VERS_C
etat_longer_bord=LONGE_VERS_C;
}
break; break;
case LONGE_VERS_A: case LONGE_NORMAL:
commande_translation_longer_vers_A(); if(longer_direction==LONGER_VERS_A){
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || commande_translation_longer_vers_A();
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
etat_longer_bord = LONGE_COLLE; (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
printf("Longer colle\n"); etat_longer_bord = LONGE_COLLE;
} }
break; }else{
commande_translation_longer_vers_C();
case LONGE_VERS_C: if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
commande_translation_longer_vers_C(); (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || etat_longer_bord = LONGE_COLLE;
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ }
etat_longer_bord = LONGE_COLLE;
printf("Longer colle\n");
} }
break; break;
case LONGE_COLLE: case LONGE_COLLE:
if(cerise_accostage() == ACTION_TERMINEE){ if(cerise_accostage() == ACTION_TERMINEE){
etat_longer_bord = LONGE_INIT; etat_longer_bord = LONGE_NORMAL;
printf("Longer INIT\n");
} }
} }
return ACTION_EN_COURS; return ACTION_EN_COURS;