From dbe8ea2706fcab258c00563f5b7affe4dbb24fa8 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 22 Jun 2024 16:36:31 +0200 Subject: [PATCH] Prepa Match 2 - Coupe IDF --- Holonome2023.c | 2 +- Strategie.c | 8 +-- Strategie_2024.c | 123 ++++++++++++++++++++++++-------------- Strategie_2024_panneaux.c | 92 ++++------------------------ Strategie_2024_panneaux.h | 2 +- Strategie_2024_pots.c | 4 +- Strategie_2024_pots.h | 2 +- 7 files changed, 100 insertions(+), 133 deletions(-) diff --git a/Holonome2023.c b/Holonome2023.c index 1e3b514..57f059e 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -110,7 +110,7 @@ int main() { break; case MATCH_EN_COURS: - if (timer_match_ms > 90000){ // 90 secondes + if (timer_match_ms > 100000){ // 90 secondes printf("MATCH_ARRET_EN_COURS\n"); statu_match = MATCH_ARRET_EN_COURS; } diff --git a/Strategie.c b/Strategie.c index 7cb0fec..8299517 100644 --- a/Strategie.c +++ b/Strategie.c @@ -479,7 +479,7 @@ enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms switch(etat_calage_debut){ case CD_ENVOI_CDE_BORDURE: - i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); + i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN); tempo_ms = 2000; etat_calage_debut = CD_LECTURE_BORDURE_Y; break; @@ -487,7 +487,7 @@ enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms case CD_LECTURE_BORDURE_Y: tempo_ms--; i2c_annexe_get_VL53L8(&validite, &angle, &distance); - if(validite == VL53L8_BORDURE){ + if(validite == VL53L8_DISTANCE_LOIN){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); commande_vitesse_stop(); Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR); @@ -510,7 +510,7 @@ enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms (0 * DEGRE_EN_RADIAN) - ANGLE_PINCE); } 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_DISTANCE_LOIN); etat_calage_debut = CD_LECTURE_BORDURE_X; tempo_ms = 2000; } @@ -519,7 +519,7 @@ enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms case CD_LECTURE_BORDURE_X: tempo_ms--; i2c_annexe_get_VL53L8(&validite, &angle, &distance); - if(validite == VL53L8_BORDURE){ + if(validite == VL53L8_DISTANCE_LOIN){ i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); commande_vitesse_stop(); if(couleur == COULEUR_BLEU){ diff --git a/Strategie_2024.c b/Strategie_2024.c index 414faba..0d221cf 100644 --- a/Strategie_2024.c +++ b/Strategie_2024.c @@ -1,4 +1,6 @@ #include "Strategie_2024.h" +#include "Localisation.h" +#include "Geometrie_robot.h" #include "pico/stdlib.h" #include "Balise_VL53L1X.h" #include "Temps.h" @@ -96,6 +98,8 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) static int tempo_ms; static int nb_plante_ok = 0; static int bras_depose = PLANTE_BRAS_1; + static int depose_en_cours = 0; + static int pre_fin_match=0; static enum { TAP_CALAGE, @@ -111,14 +115,16 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) TAP_RENTRE_RECALE, TAP_DEPOSE_0, TAP_DEPOSE_1, + TAP_PANNEAU_SOLAIRE_1, TAP_DEPOSE_2, TAP_DEPOSE_3, TAP_RECHARGE, TAP_FINI } etat_test = TAP_CALAGE; - if(temps_ms > 80000){ - etat_test = TAP_RECHARGE; + if(temps_ms > 80000 && depose_en_cours == 0){ + etat_test = TAP_RENTRE; + pre_fin_match = 1; } @@ -147,7 +153,7 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) case TAP_ALLER_PLANTE: position = get_position_approche_zone_plante(couleur, get_zone_plante(couleur)); - Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); if(Strategie_aller_a(position.x_mm, position.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){ etat_test=TAP_PLANTE_ORIENTATION; } @@ -200,6 +206,7 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) break; case TAP_RENTRE: + depose_en_cours = 1; if(couleur == COULEUR_BLEU){ angle_destination = 60 * DEGRE_EN_RADIAN; pos_x = 450; @@ -222,54 +229,68 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) break; case TAP_DEPOSE_0: + angle_destination = -30 * DEGRE_EN_RADIAN; if(couleur == COULEUR_BLEU){ pos_x = 300; - angle_destination = 15 * DEGRE_EN_RADIAN; + }else{ pos_x = 3000-300; - angle_destination = (90-(+15)) * DEGRE_EN_RADIAN; } Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - if(Strategie_aller_a_puis_tourner(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + i2c_annexe_actionneur_pot(POT_5, BRAS_LEVITE, DOIGT_TIENT); etat_test=TAP_DEPOSE_1; } break; case TAP_DEPOSE_1: commande_vitesse_stop(); - if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){ + if(couleur == COULEUR_BLEU){ + if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_2 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_PANNEAUX_SOLAIRES; + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + } + }else{ + if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_2 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_PANNEAUX_SOLAIRES; + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + } + } + break; + + case TAP_PANNEAUX_SOLAIRES: + if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){ etat_test=TAP_DEPOSE_2; } break; case TAP_DEPOSE_2: + angle_destination = 150 * DEGRE_EN_RADIAN; if(couleur == COULEUR_BLEU){ - angle_destination = (180+15) * DEGRE_EN_RADIAN; - pos_x = 450; + pos_x = 550; }else{ - angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN; - pos_x = 3000-450; + pos_x = 3000-550; } Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - if(Strategie_aller_a_puis_tourner(pos_x, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ - + if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ etat_test=TAP_DEPOSE_3; } break; case TAP_DEPOSE_3: - if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){ - etat_test=TAP_PANNEAUX_SOLAIRES; + commande_vitesse_stop(); + if(couleur == COULEUR_BLEU){ + if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_RECHARGE; + } + }else{ + if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_RECHARGE; + } } break; - - case TAP_PANNEAUX_SOLAIRES: - if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){ - etat_test=TAP_RECHARGE; - } - break; - + case TAP_RECHARGE: if(rentre_recharge(couleur, step_ms) == ACTION_TERMINEE){ etat_test=TAP_FINI; @@ -288,40 +309,54 @@ 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){ struct trajectoire_t trajectoire; enum etat_action_t etat_action; + enum validite_vl53l8_t validite; + float angle, distance, pos_x; + + static int tempo_ms; static enum { - ZONE_BASSE, - ZONE_OPPOSEE, - } etat_rentre_charge = ZONE_OPPOSEE; + RR_ORIENTE, + RR_RECALE, + RR_DEPLACE, + } etat_rentre_charge = RR_DEPLACE; 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_BASSE; - }else if(etat_action == ACTION_TERMINEE){ - return ACTION_TERMINEE; + case RR_ORIENTE: + if(Strategie_tourner_a(60 * DEGRE_EN_RADIAN, step_ms) == ACTION_TERMINEE){ + i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); + commande_vitesse_stop(); + tempo_ms = 2000; + etat_rentre_charge = RR_RECALE; } break; - case ZONE_BASSE: - if(couleur == COULEUR_BLEU){ - etat_action = Strategie_aller_a(300, 300, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); - }else{ - etat_action = Strategie_aller_a(2700, 300, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); + case RR_RECALE: + tempo_ms--; + i2c_annexe_get_VL53L8(&validite, &angle, &distance); + if(validite == VL53L8_BORDURE){ + i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); + commande_vitesse_stop(); + Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR); + etat_rentre_charge = RR_DEPLACE; } - if(etat_action == ACTION_ECHEC){ - etat_rentre_charge = ZONE_OPPOSEE; - }else if(etat_action == ACTION_TERMINEE){ + if(tempo_ms <= 0){ + etat_rentre_charge =RR_DEPLACE; + } + break; + + case RR_DEPLACE: + if(couleur == COULEUR_BLEU){ + pos_x = Localisation_get().x_mm + 30; + }else{ + pos_x = Localisation_get().x_mm - 30; + } + if( Strategie_aller_a(pos_x , 150, EVITEMENT_SANS_EVITEMENT, step_ms) == ACTION_TERMINEE){ + etat_rentre_charge =RR_ORIENTE; return ACTION_TERMINEE; } break; } return ACTION_EN_COURS; -} \ No newline at end of file +} diff --git a/Strategie_2024_panneaux.c b/Strategie_2024_panneaux.c index 80c1e55..e38d423 100644 --- a/Strategie_2024_panneaux.c +++ b/Strategie_2024_panneaux.c @@ -25,51 +25,29 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t TP_TOURNE_PANNEAU_6, TP_DEGAGEMENT, TP_DEGAGEMENT_ACTIF - } etat_tourne_panneaux = TP_APPROCHE_PANNEAU_1; + } etat_tourne_panneaux = TP_TOURNE_PANNEAU_1; switch (etat_tourne_panneaux) { - case TP_APPROCHE_PANNEAU_1: - Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - if(couleur == COULEUR_BLEU){ - etat_action = Strategie_tourner_et_aller_a( - 333, 300, (150. *DEGRE_EN_RADIAN), - EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); - }else{ - etat_action = Strategie_tourner_et_aller_a( - 3000 - 333, 300, (150. *DEGRE_EN_RADIAN), - EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms); - } - if (etat_action == ACTION_TERMINEE){ - etat_tourne_panneaux = TP_TOURNE_PANNEAU_1; - }else if(etat_action == ACTION_ECHEC){ - return ACTION_TERMINEE; - } - break; - case TP_TOURNE_PANNEAU_1: if(couleur == COULEUR_BLEU){ - etat_action = Strategie_tourner_et_aller_a( - 333, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); + etat_action = Strategie_aller_a(330, 170, EVITEMENT_SANS_EVITEMENT, step_ms); }else{ - etat_action = Strategie_tourner_et_aller_a( - 3000 - 333, 180, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); + etat_action = Strategie_aller_a(3000 - 335, 170, EVITEMENT_SANS_EVITEMENT, step_ms); } if (etat_action == ACTION_TERMINEE){ - Score_ajout_panneau(1); etat_tourne_panneaux = TP_TOURNE_PANNEAU_2; } break; case TP_TOURNE_PANNEAU_2: if(couleur == COULEUR_BLEU){ - etat_action = active_panneau_solaire(568, step_ms); + etat_action = active_panneau_solaire(578, step_ms); }else{ - etat_action = active_panneau_solaire(3000 - 543, step_ms); + etat_action = active_panneau_solaire(3000 - 578, step_ms); } if (etat_action == ACTION_TERMINEE){ - Score_ajout_panneau(1); etat_tourne_panneaux = TP_TOURNE_PANNEAU_3; }else if(etat_action == ACTION_ECHEC){ etat_tourne_panneaux = TP_DEGAGEMENT; @@ -78,65 +56,19 @@ 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(813, step_ms); + etat_action = active_panneau_solaire(812, step_ms); }else{ - etat_action = active_panneau_solaire(3000 - 768, step_ms); + etat_action = active_panneau_solaire(3000 - 802, step_ms); } if (etat_action == ACTION_TERMINEE){ - Score_ajout_panneau(1); etat_tourne_panneaux = TP_DEGAGEMENT; }else if(etat_action == ACTION_ECHEC){ etat_tourne_panneaux = TP_DEGAGEMENT; } break; - case TP_TOURNE_PANNEAU_4: - if(couleur == COULEUR_BLEU){ - Trajectoire_bezier(&trajectoire, 773, 200, 855, 310, 1323, 400, 1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); - }else{ - Trajectoire_bezier(&trajectoire, 3000-773, 200, 3000-855, 310, 3000-1323, 400, 3000-1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); - } - etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE); - if (etat_action == ACTION_TERMINEE){ - Score_ajout_panneau(1); - etat_tourne_panneaux = TP_TOURNE_PANNEAU_5; - }else if(etat_action == ACTION_ECHEC){ - etat_tourne_panneaux = TP_DEGAGEMENT; - } - break; - - case TP_TOURNE_PANNEAU_5: - if(couleur == COULEUR_BLEU){ - Trajectoire_bezier(&trajectoire, 1323, 200, 1410, 310, 1547, 400, 1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); - }else{ - Trajectoire_bezier(&trajectoire, 3000-1323, 200, 3000-1410, 310, 3000-1547, 400, 3000-1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); - } - etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE); - if (etat_action == ACTION_TERMINEE){ - Score_ajout_panneau(1); - etat_tourne_panneaux = TP_TOURNE_PANNEAU_6; - }else if(etat_action == ACTION_ECHEC){ - etat_tourne_panneaux = TP_DEGAGEMENT; - } - break; - - case TP_TOURNE_PANNEAU_6: - if(couleur == COULEUR_BLEU){ - Trajectoire_bezier(&trajectoire, 1547, 200, 1630, 310, 1771, 400, 1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); - }else{ - Trajectoire_bezier(&trajectoire, 3000-1547, 200, 3000-1630, 310, 3000-1771, 400, 3000-1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN)); - } - etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE); - if (etat_action == ACTION_TERMINEE){ - Score_ajout_panneau(1); - etat_tourne_panneaux = TP_TOURNE_PANNEAU_6; - }else if(etat_action == ACTION_ECHEC){ - etat_tourne_panneaux = TP_DEGAGEMENT; - } - break; - case TP_DEGAGEMENT: - if(Localisation_get().y_mm < 250){ + if(Localisation_get().y_mm < 360){ etat_tourne_panneaux = TP_DEGAGEMENT_ACTIF; }else{ commande_vitesse_stop(); @@ -145,7 +77,7 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t break; case TP_DEGAGEMENT_ACTIF: - return Strategie_aller_a(Localisation_get().x_mm, 250, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); + return Strategie_aller_a(Localisation_get().x_mm, 360, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms); break; } return ACTION_EN_COURS; @@ -156,9 +88,9 @@ 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)); + Localisation_get().x_mm, 400, pos_x, 400, pos_x, 150, + Localisation_get().angle_radian, + Geometrie_get_angle_optimal(Localisation_get().angle_radian, -30 * DEGRE_EN_RADIAN)); etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE); return etat_action; diff --git a/Strategie_2024_panneaux.h b/Strategie_2024_panneaux.h index 86af423..20b6386 100644 --- a/Strategie_2024_panneaux.h +++ b/Strategie_2024_panneaux.h @@ -1,3 +1,3 @@ #include "Strategie.h" -enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms); \ No newline at end of file +enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms); diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index f5e28fd..5e7b020 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -26,7 +26,7 @@ float distance_bras_correction_mm[6] = -5, -15, 0, - -10 + -25 }; enum etat_bras_t{ @@ -50,7 +50,7 @@ struct position_t position_groupe_pot[6] = {.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 = 2970, .y_mm = 1326.8, .angle_radian = 90 * DEGRE_EN_RADIAN} // bidouille : 1386.8 => 1326.8 , 2963.9 => 2970 + {.x_mm = 2970, .y_mm = 1340.8, .angle_radian = 90 * DEGRE_EN_RADIAN} // bidouille : 1386.8 => 1326.8 , 2963.9 => 2970 }; diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h index e4e063a..ccaa6a4 100644 --- a/Strategie_2024_pots.h +++ b/Strategie_2024_pots.h @@ -31,7 +31,7 @@ #define DISTANCE_APPROCHE_POT_MM 300. #define DISTANCE_ATTRAPE_POT_MM 200. -#define DISTANCE_ECHANGE_POT 50. +#define DISTANCE_ECHANGE_POT 65. struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot); enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms);