Prépa match 3
This commit is contained in:
parent
c53180ab39
commit
1b1d38d9d4
30
Strategie.c
30
Strategie.c
@ -94,7 +94,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
|
||||
|
||||
switch(etat_strategie){
|
||||
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;
|
||||
}
|
||||
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
|
||||
// 2 Si la valeur de la bordure est valide, lire l'angle et la distance.
|
||||
// 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;
|
||||
struct trajectoire_t trajectoire;
|
||||
enum etat_action_t etat_action;
|
||||
int tempo_ms;
|
||||
static int tempo_ms;
|
||||
|
||||
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_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
|
||||
}
|
||||
|
||||
etat_calage_debut = CD_ALLER_POSITION_INIT;
|
||||
etat_calage_debut = CD_ROTATION_POSITION_INIT;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
if(tempo_ms <= 0){
|
||||
etat_calage_debut=CD_ENVOI_CDE_BORDURE;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
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;
|
||||
|
@ -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(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_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);
|
||||
|
164
Strategie_2024.c
164
Strategie_2024.c
@ -34,16 +34,24 @@ enum etat_groupe_plante_t etat_groupe_plante[6]={
|
||||
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= 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= 1220, .y_mm=360, .angle_radian = 0}, // ATTENTION- spécifique bleu
|
||||
{.x_mm= 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},
|
||||
};
|
||||
|
||||
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];
|
||||
|
||||
@ -51,11 +59,11 @@ unsigned int get_groupe_pot(enum couleur_t couleur){
|
||||
if(couleur == COULEUR_BLEU){
|
||||
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 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_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_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_6, ZONE_PLANTE_5, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
|
||||
enum zone_plante_t *ordre_groupe_plante;
|
||||
int i;
|
||||
if(couleur == COULEUR_BLEU){
|
||||
@ -71,26 +79,34 @@ enum zone_plante_t get_zone_plante(enum couleur_t couleur){
|
||||
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){
|
||||
int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init;
|
||||
float angle_destination, pos_x;
|
||||
struct trajectoire_t trajectoire;
|
||||
struct position_t position;
|
||||
enum evitement_t evitement;
|
||||
enum etat_action_t etat_action=ACTION_EN_COURS;
|
||||
static int tempo_ms;
|
||||
static int nb_plante_ok = 0;
|
||||
static int bras_depose = PLANTE_BRAS_1;
|
||||
|
||||
static enum {
|
||||
TAP_CALAGE,
|
||||
TAP_PASSAGE_JAUNE,
|
||||
TAP_POT,
|
||||
TAP_PANNEAUX_SOLAIRES,
|
||||
TAP_ALLER_PLANTE,
|
||||
TAP_PLANTE_ORIENTATION,
|
||||
TAP_PLANTE_ATTRAPE_1,
|
||||
TAP_PLANTE_ATTRAPE_2,
|
||||
TAP_PLANTE_ATTRAPE,
|
||||
TAP_ECHANGE_POT,
|
||||
TAP_PLANTE_ATTRAPE_3,
|
||||
TAP_PLANTE_ATTRAPE_4,
|
||||
TAP_PLANTE_TERMINEE,
|
||||
TAP_PLANTE_TERMINEE_ECHEC,
|
||||
TAP_RENTRE,
|
||||
TAP_RENTRE_RECALE,
|
||||
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_RECHARGE,
|
||||
TAP_FINI
|
||||
} etat_test = TAP_CALAGE, next_etat_test;
|
||||
} etat_test = TAP_CALAGE;
|
||||
|
||||
if(temps_ms > 80000){
|
||||
etat_test = TAP_RECHARGE;
|
||||
}
|
||||
|
||||
|
||||
switch(etat_test){
|
||||
case TAP_CALAGE:
|
||||
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;
|
||||
}
|
||||
break;
|
||||
|
||||
case TAP_POT:
|
||||
if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){
|
||||
etat_test=TAP_ALLER_PLANTE;
|
||||
}
|
||||
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:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
pos_x = 940;
|
||||
}else{
|
||||
pos_x = 3000 - 940;
|
||||
}
|
||||
if(Strategie_aller_a(pos_x, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
|
||||
position = get_position_approche_zone_plante(couleur, get_zone_plante(couleur));
|
||||
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
||||
if(Strategie_aller_a(position.x_mm, position.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
|
||||
etat_test=TAP_PLANTE_ORIENTATION;
|
||||
}
|
||||
break;
|
||||
|
||||
case TAP_PLANTE_ORIENTATION:
|
||||
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;
|
||||
|
||||
case TAP_PLANTE_ATTRAPE_1:
|
||||
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
|
||||
case TAP_PLANTE_ATTRAPE:
|
||||
etat_action = Strat_2024_plante_dans_pot(_step_ms, bras_depose, get_zone_plante(couleur));
|
||||
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;
|
||||
}else if( etat_action == ACTION_ECHEC){
|
||||
etat_test=TAP_PLANTE_TERMINEE;
|
||||
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_test=TAP_PLANTE_TERMINEE_ECHEC;
|
||||
etat_action = ACTION_EN_COURS;
|
||||
}
|
||||
break;
|
||||
|
||||
case TAP_ECHANGE_POT:
|
||||
if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
|
||||
etat_test=TAP_PLANTE_ATTRAPE_3;
|
||||
}
|
||||
break;
|
||||
|
||||
case TAP_PLANTE_ATTRAPE_3:
|
||||
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_ATTRAPE_4;
|
||||
etat_action = ACTION_EN_COURS;
|
||||
}else if( etat_action == ACTION_ECHEC){
|
||||
etat_test=TAP_PLANTE_TERMINEE;
|
||||
etat_action = ACTION_EN_COURS;
|
||||
etat_test=TAP_PLANTE_ATTRAPE;
|
||||
}
|
||||
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;
|
||||
case TAP_PLANTE_TERMINEE_ECHEC:
|
||||
// On note que la zone plante actuelle est "occupée/epuisée"
|
||||
etat_groupe_plante[get_zone_plante(couleur)] = GROUPE_PLANTE_ECHEC;
|
||||
// On va à la zone suivante ou on rentre
|
||||
if(get_zone_plante(couleur) == ZONE_PLANTE_AUCUNE){
|
||||
etat_test=TAP_RENTRE;
|
||||
}else{
|
||||
etat_test=TAP_ALLER_PLANTE;
|
||||
}
|
||||
break;
|
||||
|
||||
case TAP_PLANTE_TERMINEE:
|
||||
etat_test=TAP_RENTRE;
|
||||
|
||||
|
||||
case TAP_RENTRE:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_destination = 60 * DEGRE_EN_RADIAN;
|
||||
pos_x = 350;
|
||||
pos_x = 450;
|
||||
}else{
|
||||
angle_destination = 60 * DEGRE_EN_RADIAN;
|
||||
pos_x = 3000-350;
|
||||
pos_x = 3000-450;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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){
|
||||
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;
|
||||
}else{
|
||||
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);
|
||||
@ -252,6 +260,12 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
|
||||
|
||||
case TAP_DEPOSE_3:
|
||||
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;
|
||||
}
|
||||
break;
|
||||
@ -276,7 +290,7 @@ enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
|
||||
enum etat_action_t etat_action;
|
||||
|
||||
static enum {
|
||||
ZONE_HAUTE,
|
||||
ZONE_BASSE,
|
||||
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);
|
||||
}
|
||||
if(etat_action == ACTION_ECHEC){
|
||||
etat_rentre_charge = ZONE_HAUTE;
|
||||
etat_rentre_charge = ZONE_BASSE;
|
||||
}else if(etat_action == ACTION_TERMINEE){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
|
||||
case ZONE_HAUTE:
|
||||
case ZONE_BASSE:
|
||||
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{
|
||||
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){
|
||||
etat_rentre_charge = ZONE_OPPOSEE;
|
||||
|
@ -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);
|
||||
if(couleur == COULEUR_BLEU){
|
||||
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);
|
||||
}else{
|
||||
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);
|
||||
}
|
||||
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:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
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{
|
||||
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){
|
||||
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:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
etat_action = active_panneau_solaire(558, step_ms);
|
||||
etat_action = active_panneau_solaire(568, step_ms);
|
||||
}else{
|
||||
etat_action = active_panneau_solaire(3000 - 483, step_ms);
|
||||
etat_action = active_panneau_solaire(3000 - 543, step_ms);
|
||||
}
|
||||
if (etat_action == ACTION_TERMINEE){
|
||||
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:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
etat_action = active_panneau_solaire(803, step_ms);
|
||||
etat_action = active_panneau_solaire(813, step_ms);
|
||||
}else{
|
||||
etat_action = active_panneau_solaire(3000 - 738, step_ms);
|
||||
etat_action = active_panneau_solaire(3000 - 768, step_ms);
|
||||
}
|
||||
if (etat_action == ACTION_TERMINEE){
|
||||
Score_ajout_panneau(1);
|
||||
|
@ -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_plante = Geometrie_deplace(position_robot, distance_mm);
|
||||
if( !est_dans_zone(position_plante, zone_plante)){
|
||||
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
|
||||
if(entree_dans_zone == true){
|
||||
if(robot_dans_zone == false){
|
||||
// Le robot est sorti de la zone
|
||||
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
}
|
||||
@ -121,14 +123,15 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
|
||||
tempo_asserv--;
|
||||
if(tempo_asserv <= 0){
|
||||
commande_vitesse(0, 0, 0);
|
||||
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
}
|
||||
|
||||
// 3 on asservi
|
||||
commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P;
|
||||
if(commande_vitesse_plante > 300){
|
||||
commande_vitesse_plante = 300;
|
||||
if(commande_vitesse_plante > 150){
|
||||
commande_vitesse_plante = 150;
|
||||
}
|
||||
if(commande_vitesse_plante <= 0){
|
||||
commande_vitesse_stop();
|
||||
@ -146,6 +149,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
|
||||
}else{
|
||||
tempo_ms--;
|
||||
if(tempo_ms <= 0){
|
||||
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
||||
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){
|
||||
etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
|
||||
}else if (etat_action == ACTION_ECHEC){
|
||||
i2c_annexe_ouvre_doigt_plante();
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
break;
|
||||
|
@ -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 = 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 = 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_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
|
||||
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],
|
||||
SANS_EVITEMENT, step_ms);*/
|
||||
etat_action = Strategie_aller_a_puis_tourner(
|
||||
position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN),
|
||||
|
||||
etat_action = Strategie_aller_a(
|
||||
position_approche_pot.x_mm, position_approche_pot.y_mm,
|
||||
EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
|
||||
if (etat_action == ACTION_TERMINEE){
|
||||
etat_attrape_pot = AP_ORIENTE;
|
||||
|
@ -42,6 +42,7 @@ int test_strategie_2024(){
|
||||
printf("G - Pseudo homologation.\n");
|
||||
printf("H - reglage pots.\n");
|
||||
printf("I - echange pots.\n");
|
||||
printf("J - Calage fin.\n");
|
||||
|
||||
int lettre;
|
||||
do{
|
||||
@ -92,6 +93,8 @@ int test_strategie_2024(){
|
||||
case 'I':
|
||||
while(test_echange_pot());
|
||||
break;
|
||||
|
||||
|
||||
|
||||
|
||||
case 'q':
|
||||
@ -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);
|
||||
@ -768,7 +771,7 @@ int test_attrape_1_pot(void){
|
||||
}
|
||||
switch(etat_test){
|
||||
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;
|
||||
}
|
||||
break;
|
||||
|
Loading…
Reference in New Issue
Block a user