Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Prépa match 3

Browse Source
This commit is contained in:
Samuel 2024-05-10 10:11:30 +02:00
parent c53180ab39
commit 1b1d38d9d4
7 changed files with 120 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Strategie.c
View File

@ -94,7 +94,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
switch(etat_strategie){ switch(etat_strategie){
case STRATEGIE_INIT: case STRATEGIE_INIT:
if(Strategie_calage_debut(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){ if(Strategie_calage_bas(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN; etat_strategie = STRATEGIE_FIN;
} }
break; break;
@ -455,7 +455,7 @@ enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_
} }
} }
enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms){
// 1 Envoyer la commande pour détecter la bordure // 1 Envoyer la commande pour détecter la bordure
// 2 Si la valeur de la bordure est valide, lire l'angle et la distance. // 2 Si la valeur de la bordure est valide, lire l'angle et la distance.
// Recaler la distance Y // Recaler la distance Y
@ -473,7 +473,7 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
enum validite_vl53l8_t validite; enum validite_vl53l8_t validite;
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
enum etat_action_t etat_action; enum etat_action_t etat_action;
int tempo_ms; static int tempo_ms;
float angle, distance; float angle, distance;
@ -529,34 +529,14 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR)); Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
} }
etat_calage_debut = CD_ROTATION_POSITION_INIT;
etat_calage_debut = CD_ALLER_POSITION_INIT; return ACTION_TERMINEE;
} }
if(tempo_ms <= 0){ if(tempo_ms <= 0){
etat_calage_debut=CD_ENVOI_CDE_BORDURE; etat_calage_debut=CD_ENVOI_CDE_BORDURE;
return ACTION_ECHEC; return ACTION_ECHEC;
} }
break; break;
case CD_ALLER_POSITION_INIT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(couleur == COULEUR_BLEU){
etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
}else{
etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
}
if(etat_action == ACTION_TERMINEE){
etat_calage_debut = CD_ROTATION_POSITION_INIT;
}
break;
case CD_ROTATION_POSITION_INIT:
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
/*Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
(45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);*/
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
0);
return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
//etat_calage_debut = CD_ENVOI_CDE_BORDURE;
} }
return ACTION_EN_COURS; return ACTION_EN_COURS;

2
Strategie.h
View File

@ -65,7 +65,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_calage_debut(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, enum evitement_t evitement, uint32_t step_ms); enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, enum evitement_t evitement, uint32_t step_ms);
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms); enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);

162
Strategie_2024.c
View File

@ -34,16 +34,24 @@ enum etat_groupe_plante_t etat_groupe_plante[6]={
GROUPE_PLANTE_DISPO GROUPE_PLANTE_DISPO
}; };
struct position_t approche_groupe_plante[6]={ struct position_t approche_groupe_plante_bleu[6]={
{.x_mm= 0, .y_mm=0, .angle_radian = 0}, {.x_mm= 0, .y_mm=0, .angle_radian = 0},
{.x_mm= 580, .y_mm=1530, .angle_radian = 0}, {.x_mm= 580, .y_mm=1530, .angle_radian = 0},
{.x_mm= 580, .y_mm=530, .angle_radian = 0}, {.x_mm= 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 580, .y_mm=530, .angle_radian = 0}, {.x_mm= 1220, .y_mm=360, .angle_radian = 0},
{.x_mm= 1220, .y_mm=360, .angle_radian = 0}, // ATTENTION- spécifique bleu
{.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0}, {.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
};
struct position_t approche_groupe_plante_jaune[6]={
{.x_mm= 0, .y_mm=0, .angle_radian = 0},
{.x_mm= 580, .y_mm=1530, .angle_radian = 0},
{.x_mm= 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 3000 - 1220, .y_mm=360, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
{.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
};
}
int ordre_groupe_pot[6]; int ordre_groupe_pot[6];
@ -51,11 +59,11 @@ unsigned int get_groupe_pot(enum couleur_t couleur){
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
return GROUPE_POT_L1; return GROUPE_POT_L1;
} }
return GROUPE_POT_R2; return GROUPE_POT_R1;
} }
enum zone_plante_t get_zone_plante(enum couleur_t couleur){ enum zone_plante_t get_zone_plante(enum couleur_t couleur){
enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_2, ZONE_PLANTE_3, ZONE_PLANTE_4, ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_1}; enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_2, ZONE_PLANTE_3, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_4, ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_1}; enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_6, ZONE_PLANTE_5, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
enum zone_plante_t *ordre_groupe_plante; enum zone_plante_t *ordre_groupe_plante;
int i; int i;
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
@ -71,26 +79,34 @@ enum zone_plante_t get_zone_plante(enum couleur_t couleur){
return ZONE_PLANTE_AUCUNE; return ZONE_PLANTE_AUCUNE;
} }
struct position_t get_position_approche_zone_plante(enum couleur_t couleur, enum zone_plante_t zone_plante){
if(couleur == COULEUR_BLEU){
return approche_groupe_plante_bleu[zone_plante];
}
return approche_groupe_plante_jaune[zone_plante];
}
void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init; int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init;
float angle_destination, pos_x; float angle_destination, pos_x;
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
struct position_t position;
enum evitement_t evitement; enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS; enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms; static int tempo_ms;
static int nb_plante_ok = 0;
static int bras_depose = PLANTE_BRAS_1;
static enum { static enum {
TAP_CALAGE, TAP_CALAGE,
TAP_PASSAGE_JAUNE,
TAP_POT, TAP_POT,
TAP_PANNEAUX_SOLAIRES, TAP_PANNEAUX_SOLAIRES,
TAP_ALLER_PLANTE, TAP_ALLER_PLANTE,
TAP_PLANTE_ORIENTATION, TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE_1, TAP_PLANTE_ATTRAPE,
TAP_PLANTE_ATTRAPE_2,
TAP_ECHANGE_POT, TAP_ECHANGE_POT,
TAP_PLANTE_ATTRAPE_3, TAP_PLANTE_TERMINEE_ECHEC,
TAP_PLANTE_ATTRAPE_4,
TAP_PLANTE_TERMINEE,
TAP_RENTRE, TAP_RENTRE,
TAP_RENTRE_RECALE, TAP_RENTRE_RECALE,
TAP_DEPOSE_0, TAP_DEPOSE_0,
@ -99,105 +115,97 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
TAP_DEPOSE_3, TAP_DEPOSE_3,
TAP_RECHARGE, TAP_RECHARGE,
TAP_FINI TAP_FINI
} etat_test = TAP_CALAGE, next_etat_test; } etat_test = TAP_CALAGE;
if(temps_ms > 80000){
etat_test = TAP_RECHARGE;
}
switch(etat_test){ switch(etat_test){
case TAP_CALAGE: case TAP_CALAGE:
if(Strategie_calage_debut_manuel(couleur, _step_ms) == ACTION_TERMINEE){ if(Strategie_calage_debut_manuel(couleur, _step_ms) == ACTION_TERMINEE){
if(couleur == COULEUR_BLEU){
etat_test=TAP_POT;
}else{
etat_test=TAP_PASSAGE_JAUNE;
}
}
break;
case TAP_PASSAGE_JAUNE:
if(Strategie_aller_a(2600, 1600, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT; etat_test=TAP_POT;
} }
break; break;
case TAP_POT: case TAP_POT:
if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){ if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){
etat_test=TAP_ALLER_PLANTE; etat_test=TAP_ALLER_PLANTE;
} }
break; break;
case TAP_PANNEAUX_SOLAIRES:
if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
etat_test=TAP_ALLER_PLANTE;
}
break;
case TAP_ALLER_PLANTE: case TAP_ALLER_PLANTE:
if(couleur == COULEUR_BLEU){ position = get_position_approche_zone_plante(couleur, get_zone_plante(couleur));
pos_x = 940; Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
}else{ if(Strategie_aller_a(position.x_mm, position.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
pos_x = 3000 - 940;
}
if(Strategie_aller_a(pos_x, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ORIENTATION; etat_test=TAP_PLANTE_ORIENTATION;
} }
break; break;
case TAP_PLANTE_ORIENTATION: case TAP_PLANTE_ORIENTATION:
if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){ if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_1; etat_test=TAP_PLANTE_ATTRAPE;
} }
break; break;
case TAP_PLANTE_ATTRAPE_1: case TAP_PLANTE_ATTRAPE:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur)); etat_action = Strat_2024_plante_dans_pot(_step_ms, bras_depose, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){ if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_2; etat_test=TAP_PLANTE_ATTRAPE;
nb_plante_ok++;
if(bras_depose == PLANTE_BRAS_1){
bras_depose = PLANTE_BRAS_6;
}else{
bras_depose = PLANTE_BRAS_1;
}
if(nb_plante_ok == 2){
etat_test = TAP_ECHANGE_POT;
}
if(nb_plante_ok == 4){
etat_test = TAP_RENTRE;
}
etat_action = ACTION_EN_COURS; etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){ }else if( etat_action == ACTION_ECHEC){
etat_test=TAP_PLANTE_TERMINEE; etat_test=TAP_PLANTE_TERMINEE_ECHEC;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_2:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_ECHANGE_POT;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_PLANTE_TERMINEE;
etat_action = ACTION_EN_COURS; etat_action = ACTION_EN_COURS;
} }
break; break;
case TAP_ECHANGE_POT: case TAP_ECHANGE_POT:
if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){ if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_3; etat_test=TAP_PLANTE_ATTRAPE;
} }
break; break;
case TAP_PLANTE_ATTRAPE_3: case TAP_PLANTE_TERMINEE_ECHEC:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur)); // On note que la zone plante actuelle est "occupée/epuisée"
if( etat_action == ACTION_TERMINEE){ etat_groupe_plante[get_zone_plante(couleur)] = GROUPE_PLANTE_ECHEC;
etat_test=TAP_PLANTE_ATTRAPE_4; // On va à la zone suivante ou on rentre
etat_action = ACTION_EN_COURS; if(get_zone_plante(couleur) == ZONE_PLANTE_AUCUNE){
}else if( etat_action == ACTION_ECHEC){ etat_test=TAP_RENTRE;
etat_test=TAP_PLANTE_TERMINEE; }else{
etat_action = ACTION_EN_COURS; etat_test=TAP_ALLER_PLANTE;
} }
break; break;
case TAP_PLANTE_ATTRAPE_4:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_TERMINEE;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_PLANTE_TERMINEE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_TERMINEE:
etat_test=TAP_RENTRE;
case TAP_RENTRE: case TAP_RENTRE:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
angle_destination = 60 * DEGRE_EN_RADIAN; angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 350; pos_x = 450;
}else{ }else{
angle_destination = 60 * DEGRE_EN_RADIAN; angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 3000-350; pos_x = 3000-450;
} }
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
@ -207,7 +215,7 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
break; break;
case TAP_RENTRE_RECALE: case TAP_RENTRE_RECALE:
etat_action = Strategie_calage_debut(couleur, step_ms); etat_action = Strategie_calage_bas(couleur, step_ms);
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){ if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
etat_test=TAP_DEPOSE_0; etat_test=TAP_DEPOSE_0;
} }
@ -219,7 +227,7 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
angle_destination = 15 * DEGRE_EN_RADIAN; angle_destination = 15 * DEGRE_EN_RADIAN;
}else{ }else{
pos_x = 3000-300; pos_x = 3000-300;
angle_destination = (90-15) * DEGRE_EN_RADIAN; angle_destination = (90-(+15)) * DEGRE_EN_RADIAN;
} }
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
@ -252,6 +260,12 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
case TAP_DEPOSE_3: case TAP_DEPOSE_3:
if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){ if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
etat_test=TAP_PANNEAUX_SOLAIRES;
}
break;
case TAP_PANNEAUX_SOLAIRES:
if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
etat_test=TAP_RECHARGE; etat_test=TAP_RECHARGE;
} }
break; break;
@ -276,7 +290,7 @@ enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
enum etat_action_t etat_action; enum etat_action_t etat_action;
static enum { static enum {
ZONE_HAUTE, ZONE_BASSE,
ZONE_OPPOSEE, ZONE_OPPOSEE,
} etat_rentre_charge = ZONE_OPPOSEE; } etat_rentre_charge = ZONE_OPPOSEE;
@ -290,17 +304,17 @@ enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
etat_action = Strategie_aller_a(300, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); etat_action = Strategie_aller_a(300, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
} }
if(etat_action == ACTION_ECHEC){ if(etat_action == ACTION_ECHEC){
etat_rentre_charge = ZONE_HAUTE; etat_rentre_charge = ZONE_BASSE;
}else if(etat_action == ACTION_TERMINEE){ }else if(etat_action == ACTION_TERMINEE){
return ACTION_TERMINEE; return ACTION_TERMINEE;
} }
break; break;
case ZONE_HAUTE: case ZONE_BASSE:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
etat_action = Strategie_aller_a(300, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); etat_action = Strategie_aller_a(300, 300, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}else{ }else{
etat_action = Strategie_aller_a(2700, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); etat_action = Strategie_aller_a(2700, 300, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
} }
if(etat_action == ACTION_ECHEC){ if(etat_action == ACTION_ECHEC){
etat_rentre_charge = ZONE_OPPOSEE; etat_rentre_charge = ZONE_OPPOSEE;

16
Strategie_2024_panneaux.c
View File

@ -34,11 +34,11 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
etat_action = Strategie_tourner_et_aller_a( etat_action = Strategie_tourner_et_aller_a(
323, 300, (150. *DEGRE_EN_RADIAN), 333, 300, (150. *DEGRE_EN_RADIAN),
EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}else{ }else{
etat_action = Strategie_tourner_et_aller_a( etat_action = Strategie_tourner_et_aller_a(
3000 - 173, 300, (150. *DEGRE_EN_RADIAN), 3000 - 333, 300, (150. *DEGRE_EN_RADIAN),
EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
} }
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
@ -51,10 +51,10 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
case TP_TOURNE_PANNEAU_1: case TP_TOURNE_PANNEAU_1:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
etat_action = Strategie_tourner_et_aller_a( etat_action = Strategie_tourner_et_aller_a(
323, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); 333, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
}else{ }else{
etat_action = Strategie_tourner_et_aller_a( etat_action = Strategie_tourner_et_aller_a(
3000 - 248, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); 3000 - 333, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
} }
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
Score_ajout_panneau(1); Score_ajout_panneau(1);
@ -64,9 +64,9 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
case TP_TOURNE_PANNEAU_2: case TP_TOURNE_PANNEAU_2:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
etat_action = active_panneau_solaire(558, step_ms); etat_action = active_panneau_solaire(568, step_ms);
}else{ }else{
etat_action = active_panneau_solaire(3000 - 483, step_ms); etat_action = active_panneau_solaire(3000 - 543, step_ms);
} }
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
Score_ajout_panneau(1); Score_ajout_panneau(1);
@ -78,9 +78,9 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
case TP_TOURNE_PANNEAU_3: case TP_TOURNE_PANNEAU_3:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
etat_action = active_panneau_solaire(803, step_ms); etat_action = active_panneau_solaire(813, step_ms);
}else{ }else{
etat_action = active_panneau_solaire(3000 - 738, step_ms); etat_action = active_panneau_solaire(3000 - 768, step_ms);
} }
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
Score_ajout_panneau(1); Score_ajout_panneau(1);

9
Strategie_2024_plante.c
View File

@ -93,12 +93,14 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
position_robot.angle_radian += ANGLE_PINCE + angle_rad; position_robot.angle_radian += ANGLE_PINCE + angle_rad;
position_plante = Geometrie_deplace(position_robot, distance_mm); position_plante = Geometrie_deplace(position_robot, distance_mm);
if( !est_dans_zone(position_plante, zone_plante)){ if( !est_dans_zone(position_plante, zone_plante)){
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_ECHEC; return ACTION_ECHEC;
} }
robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante); robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
if(entree_dans_zone == true){ if(entree_dans_zone == true){
if(robot_dans_zone == false){ if(robot_dans_zone == false){
// Le robot est sorti de la zone // Le robot est sorti de la zone
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_ECHEC; return ACTION_ECHEC;
} }
} }
@ -121,14 +123,15 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
tempo_asserv--; tempo_asserv--;
if(tempo_asserv <= 0){ if(tempo_asserv <= 0){
commande_vitesse(0, 0, 0); commande_vitesse(0, 0, 0);
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_ECHEC; return ACTION_ECHEC;
} }
} }
// 3 on asservi // 3 on asservi
commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P; commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P;
if(commande_vitesse_plante > 300){ if(commande_vitesse_plante > 150){
commande_vitesse_plante = 300; commande_vitesse_plante = 150;
} }
if(commande_vitesse_plante <= 0){ if(commande_vitesse_plante <= 0){
commande_vitesse_stop(); commande_vitesse_stop();
@ -146,6 +149,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
}else{ }else{
tempo_ms--; tempo_ms--;
if(tempo_ms <= 0){ if(tempo_ms <= 0){
etat_aller_a_plante = SAAP_INIT_DETECTION;
return ACTION_ECHEC; return ACTION_ECHEC;
} }
} }
@ -187,6 +191,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE; etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
}else if (etat_action == ACTION_ECHEC){ }else if (etat_action == ACTION_ECHEC){
i2c_annexe_ouvre_doigt_plante();
return ACTION_ECHEC; return ACTION_ECHEC;
} }
break; break;

12
Strategie_2024_pots.c
View File

@ -47,10 +47,10 @@ struct position_t position_groupe_pot[6] =
{ {
{.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN}, {.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN},
{.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN}, {.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN},
{.x_mm = 1020, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, {.x_mm = 1020, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // 1020 : bidouille
{.x_mm = 1980, .y_mm = 51.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // Attention bidouille !!! {.x_mm = 1980, .y_mm = 51.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // Attention bidouille !!!
{.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN}, {.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN},
{.x_mm = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN} {.x_mm = 2970, .y_mm = 1326.8, .angle_radian = 90 * DEGRE_EN_RADIAN} // bidouille : 1386.8 => 1326.8 , 2963.9 => 2970
}; };
@ -138,11 +138,9 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM); position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras])); position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
/*etat_action = Strategie_tourner_et_aller_a(
position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0], etat_action = Strategie_aller_a(
SANS_EVITEMENT, step_ms);*/ position_approche_pot.x_mm, position_approche_pot.y_mm,
etat_action = Strategie_aller_a_puis_tourner(
position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN),
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
etat_attrape_pot = AP_ORIENTE; etat_attrape_pot = AP_ORIENTE;

7
Test_strategie_2024.c
View File

@ -42,6 +42,7 @@ int test_strategie_2024(){
printf("G - Pseudo homologation.\n"); printf("G - Pseudo homologation.\n");
printf("H - reglage pots.\n"); printf("H - reglage pots.\n");
printf("I - echange pots.\n"); printf("I - echange pots.\n");
printf("J - Calage fin.\n");
int lettre; int lettre;
do{ do{
@ -94,6 +95,8 @@ int test_strategie_2024(){
break; break;
case 'q': case 'q':
case 'Q': case 'Q':
return 0; return 0;
@ -193,7 +196,7 @@ int test_calage_debut(){
} }
} }
etat_action = Strategie_calage_debut(COULEUR_BLEU, _step_ms); etat_action = Strategie_calage_bas(COULEUR_BLEU, _step_ms);
} }
lettre = getchar_timeout_us(0); lettre = getchar_timeout_us(0);
@ -768,7 +771,7 @@ int test_attrape_1_pot(void){
} }
switch(etat_test){ switch(etat_test){
case TAP_CALAGE: case TAP_CALAGE:
if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){ if(Strategie_calage_bas(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_APPROCHE_POT; etat_test=TAP_APPROCHE_POT;
} }
break; break;