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

Match 1

Browse Source
This commit is contained in:
Samuel 2024-05-09 12:34:16 +02:00
parent cf1e4719d3
commit 9892e5d923
6 changed files with 133 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Holonome2023.c
View File

@ -110,7 +110,7 @@ int main() {
break; break;
case MATCH_EN_COURS: case MATCH_EN_COURS:
if (timer_match_ms > 98000){ // 98 secondes if (timer_match_ms > 90000){ // 98 secondes
printf("MATCH_ARRET_EN_COURS\n"); printf("MATCH_ARRET_EN_COURS\n");
statu_match = MATCH_ARRET_EN_COURS; statu_match = MATCH_ARRET_EN_COURS;
} }
@ -119,9 +119,6 @@ int main() {
case MATCH_ARRET_EN_COURS: case MATCH_ARRET_EN_COURS:
commande_vitesse_stop(); commande_vitesse_stop();
if(Robot_est_dans_zone_depose(couleur)){
Score_set_pieds_dans_plat();
}
if (timer_match_ms > 100000){ // 100 secondes if (timer_match_ms > 100000){ // 100 secondes
statu_match = MATCH_TERMINEE; statu_match = MATCH_TERMINEE;

17
Strategie.c
View File

@ -473,17 +473,19 @@ 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;
float angle, distance; float angle, distance;
switch(etat_calage_debut){ switch(etat_calage_debut){
case CD_ENVOI_CDE_BORDURE: case CD_ENVOI_CDE_BORDURE:
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
tempo_ms = 2000;
etat_calage_debut = CD_LECTURE_BORDURE_Y; etat_calage_debut = CD_LECTURE_BORDURE_Y;
break; break;
case CD_LECTURE_BORDURE_Y: case CD_LECTURE_BORDURE_Y:
tempo_ms--;
i2c_annexe_get_VL53L8(&validite, &angle, &distance); i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_BORDURE){ if(validite == VL53L8_BORDURE){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
@ -492,6 +494,10 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle); Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
etat_calage_debut = CD_ROTATION_VERS_X; etat_calage_debut = CD_ROTATION_VERS_X;
} }
if(tempo_ms <= 0){
etat_calage_debut=CD_ENVOI_CDE_BORDURE;
return ACTION_ECHEC;
}
break; break;
case CD_ROTATION_VERS_X: case CD_ROTATION_VERS_X:
@ -506,10 +512,12 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
etat_calage_debut = CD_LECTURE_BORDURE_X; etat_calage_debut = CD_LECTURE_BORDURE_X;
tempo_ms = 2000;
} }
break; break;
case CD_LECTURE_BORDURE_X: case CD_LECTURE_BORDURE_X:
tempo_ms--;
i2c_annexe_get_VL53L8(&validite, &angle, &distance); i2c_annexe_get_VL53L8(&validite, &angle, &distance);
if(validite == VL53L8_BORDURE){ if(validite == VL53L8_BORDURE){
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
@ -524,6 +532,10 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
etat_calage_debut = CD_ALLER_POSITION_INIT; etat_calage_debut = CD_ALLER_POSITION_INIT;
} }
if(tempo_ms <= 0){
etat_calage_debut=CD_ENVOI_CDE_BORDURE;
return ACTION_ECHEC;
}
break; break;
case CD_ALLER_POSITION_INIT: case CD_ALLER_POSITION_INIT:
@ -597,4 +609,5 @@ enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_
} }
return ACTION_EN_COURS; return ACTION_EN_COURS;
} }

95
Strategie_2024.c
View File

@ -62,7 +62,7 @@ enum zone_plante_t get_zone_plante(enum couleur_t couleur){
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; float angle_destination, pos_x;
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
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;
@ -80,11 +80,14 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
TAP_PLANTE_ATTRAPE_3, TAP_PLANTE_ATTRAPE_3,
TAP_PLANTE_ATTRAPE_4, TAP_PLANTE_ATTRAPE_4,
TAP_RENTRE, TAP_RENTRE,
TAP_RENTRE_RECALE,
TAP_DEPOSE_0,
TAP_DEPOSE_1, TAP_DEPOSE_1,
TAP_DEPOSE_2, TAP_DEPOSE_2,
TAP_DEPOSE_3, TAP_DEPOSE_3,
TAP_RECHARGE,
TAP_FINI TAP_FINI
} etat_test = TAP_CALAGE; } etat_test = TAP_CALAGE, next_etat_test;
switch(etat_test){ switch(etat_test){
@ -106,7 +109,12 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
break; break;
case TAP_ALLER_PLANTE: case TAP_ALLER_PLANTE:
if(Strategie_aller_a(940, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){ 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){
etat_test=TAP_PLANTE_ORIENTATION; etat_test=TAP_PLANTE_ORIENTATION;
} }
break; break;
@ -169,13 +177,37 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
case TAP_RENTRE: case TAP_RENTRE:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 350;
}else{
angle_destination = 60 * DEGRE_EN_RADIAN;
pos_x = 3000-350;
}
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_RENTRE_RECALE;
}
break;
case TAP_RENTRE_RECALE:
etat_action = Strategie_calage_debut(couleur, step_ms);
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
etat_test=TAP_DEPOSE_0;
}
break;
case TAP_DEPOSE_0:
if(couleur == COULEUR_BLEU){
pos_x = 300;
angle_destination = 15 * DEGRE_EN_RADIAN; angle_destination = 15 * DEGRE_EN_RADIAN;
}else{ }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); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(300, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ if(Strategie_aller_a_puis_tourner(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE_1; etat_test=TAP_DEPOSE_1;
} }
break; break;
@ -190,25 +222,76 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms)
case TAP_DEPOSE_2: case TAP_DEPOSE_2:
if(couleur == COULEUR_BLEU){ if(couleur == COULEUR_BLEU){
angle_destination = (180+15) * DEGRE_EN_RADIAN; angle_destination = (180+15) * DEGRE_EN_RADIAN;
pos_x = 450;
}else{ }else{
angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN; angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN;
pos_x = 3000-450;
} }
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a_puis_tourner(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ if(Strategie_aller_a_puis_tourner(pos_x, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE_3; etat_test=TAP_DEPOSE_3;
} }
break; break;
case TAP_DEPOSE_3: case TAP_DEPOSE_3:
commande_vitesse_stop();
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_RECHARGE;
}
break;
case TAP_RECHARGE:
if(rentre_recharge(couleur, step_ms) == ACTION_TERMINEE){
etat_test=TAP_FINI; etat_test=TAP_FINI;
} }
break; break;
case TAP_FINI: case TAP_FINI:
commande_vitesse_stop();
break; break;
} }
}
enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
struct trajectoire_t trajectoire;
enum etat_action_t etat_action;
static enum {
ZONE_HAUTE,
ZONE_OPPOSEE,
} etat_rentre_charge = ZONE_OPPOSEE;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
switch (etat_rentre_charge){
case ZONE_OPPOSEE:
if(couleur == COULEUR_BLEU){
etat_action = Strategie_aller_a(2700, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}else{
etat_action = Strategie_aller_a(300, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}
if(etat_action == ACTION_ECHEC){
etat_rentre_charge = ZONE_HAUTE;
}else if(etat_action == ACTION_TERMINEE){
return ACTION_TERMINEE;
}
break;
case ZONE_HAUTE:
if(couleur == COULEUR_BLEU){
etat_action = Strategie_aller_a(300, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}else{
etat_action = Strategie_aller_a(2700, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
}
if(etat_action == ACTION_ECHEC){
etat_rentre_charge = ZONE_OPPOSEE;
}else if(etat_action == ACTION_TERMINEE){
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
} }

1
Strategie_2024.h
View File

@ -13,6 +13,7 @@
#define STRATEGIE_2024_H #define STRATEGIE_2024_H
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);
enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms);
// STRATEGIE_H // STRATEGIE_H
#endif #endif

35
Strategie_2024_panneaux.c
View File

@ -7,6 +7,8 @@
#include "Localisation.h" #include "Localisation.h"
#include "Score.h" #include "Score.h"
enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms);
/// @brief Fonction demande au robot d'aller activer les panneaux solaires /// @brief Fonction demande au robot d'aller activer les panneaux solaires
enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms){
enum etat_action_t etat_action; enum etat_action_t etat_action;
@ -36,7 +38,7 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
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 - 323, 300, (150. *DEGRE_EN_RADIAN), 3000 - 173, 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){
@ -49,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, 200, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); 323, 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 - 323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); 3000 - 173, 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);
@ -62,14 +64,13 @@ 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){
Trajectoire_bezier(&trajectoire, 323, 200, 405, 310, 548, 400, 548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); etat_action = active_panneau_solaire(558, step_ms);
}else{ }else{
Trajectoire_bezier(&trajectoire, 3000-323, 200, 3000-405, 310, 3000-548, 400, 3000-548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); etat_action = active_panneau_solaire(3000 - 408, step_ms);
} }
etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
Score_ajout_panneau(1); Score_ajout_panneau(1);
etat_tourne_panneaux = TP_DEGAGEMENT; etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
}else if(etat_action == ACTION_ECHEC){ }else if(etat_action == ACTION_ECHEC){
etat_tourne_panneaux = TP_DEGAGEMENT; etat_tourne_panneaux = TP_DEGAGEMENT;
} }
@ -77,14 +78,13 @@ 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){
Trajectoire_bezier(&trajectoire, 548, 200, 630, 310, 773, 400, 773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); etat_action = active_panneau_solaire(803, step_ms);
}else{ }else{
Trajectoire_bezier(&trajectoire, 3000-548, 200, 3000-630, 310, 3000-773, 400, 3000-773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); etat_action = active_panneau_solaire(3000 - 653, step_ms);
} }
etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
if (etat_action == ACTION_TERMINEE){ if (etat_action == ACTION_TERMINEE){
Score_ajout_panneau(1); Score_ajout_panneau(1);
etat_tourne_panneaux = TP_TOURNE_PANNEAU_4; etat_tourne_panneaux = TP_DEGAGEMENT;
}else if(etat_action == ACTION_ECHEC){ }else if(etat_action == ACTION_ECHEC){
etat_tourne_panneaux = TP_DEGAGEMENT; etat_tourne_panneaux = TP_DEGAGEMENT;
} }
@ -149,4 +149,17 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
break; break;
} }
return ACTION_EN_COURS; return ACTION_EN_COURS;
}
enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms){
struct trajectoire_t trajectoire;
enum etat_action_t etat_action;
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, 400, pos_x, 400, pos_x, 180,
Geometrie_get_angle_optimal(Localisation_get().angle_radian, 150. *DEGRE_EN_RADIAN),
Geometrie_get_angle_optimal(Localisation_get().angle_radian, 150. *DEGRE_EN_RADIAN));
etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
return etat_action;
} }

7
Strategie_2024_pots.c
View File

@ -22,11 +22,11 @@ float angle_bras[6] =
float distance_bras_correction_mm[6] = float distance_bras_correction_mm[6] =
{ {
-12, -12,
0, -7,
-5, -5,
-15, -15,
0, 0,
0 -10
}; };
enum etat_bras_t{ enum etat_bras_t{
@ -48,7 +48,7 @@ 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},
{.x_mm = 2000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, {.x_mm = 1980, .y_mm = 41.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 = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN}
}; };
@ -146,7 +146,6 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
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;
i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
} }
break; break;