diff --git a/Holonome2023.c b/Holonome2023.c index 69a154a..8c80ee0 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -82,7 +82,7 @@ int main() { AsserMoteur_Init(); Localisation_init(); - while(mode_test()); + //while(mode_test()); i2c_maitre_init(); Trajet_init(); Balise_VL53L1X_init(); diff --git a/Strategie.c b/Strategie.c index a1b9df5..f05e53a 100644 --- a/Strategie.c +++ b/Strategie.c @@ -85,7 +85,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS}; struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; - struct objectif_t objectif_4 = { .priorite = 4, .etat = A_FAIRE, .cible = CERISE_DROITE}; + struct objectif_t objectif_4 = { .priorite = 4, .etat = FAIT, .cible = CERISE_DROITE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; objectifs[2]= objectif_3; @@ -94,8 +94,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS}; struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT}; - struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_DROITE}; - struct objectif_t objectif_4 = { .priorite = 4, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; + struct objectif_t objectif_3 = { .priorite = 3, .etat = FAIT, .cible = CERISE_DROITE}; + struct objectif_t objectif_4 = { .priorite = 4, .etat = FAIT, .cible = CERISE_GAUCHE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; objectifs[2]= objectif_3; @@ -178,12 +178,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ break; case ALLER_CERISE_GAUCHE: + Trajet_config(300, 250); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); if(couleur == COULEUR_BLEU){ Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 740, 3000 - 550, 510, 3000 - 1580, - 180, 3000 - (1500 - 45), + 180, 1500, Localisation_get().angle_radian, angle_fin); }else{ Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, @@ -196,17 +197,14 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; + Trajet_config(500, 500); } break; case ATTRAPER_CERISE_GAUCHE: - // 1 accoster - // 2 Longer en aspirant - // 3 avancer en aspirant - // 4 Revenir au milieu - // 5 accoster - // 6 longer en aspirant - // 7 avancer en aspirant + if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){ + etat_strategie = ALLER_PANIER; + } break; case ALLER_PANIER: @@ -243,10 +241,10 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ etat_strategie = ALLER_CERISE_HAUT; break; case CERISE_GAUCHE: - etat_strategie = STRATEGIE_FIN; + etat_strategie = ALLER_CERISE_GAUCHE; break; case CERISE_DROITE: - etat_strategie = STRATEGIE_FIN; + etat_strategie = ALLER_CERISE_DROITE; break; } } @@ -272,14 +270,16 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step if(Robot_est_dans_quart_haut_gauche()){ Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 465,2830, - +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); + Localisation_get().angle_radian, + Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); }else{ // Sinon, on a une courbe de bézier Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 465, 857, 465,2830, - +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); + Localisation_get().angle_radian, + Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); } }else{ // COULEUR_VERT // Si le robot est déjà dans le quart haut droit, @@ -356,7 +356,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste break; case RECULE_PANIER: - Trajet_config(250, 500); + Trajet_config(120, 250); if(couleur == COULEUR_BLEU){ point_x = 180; point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; @@ -372,6 +372,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = LANCE_DANS_PANIER; + Trajet_config(500, 500); } break; diff --git a/i2c_annexe.c b/i2c_annexe.c index 798e596..a934abc 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -73,8 +73,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){ } void i2c_annexe_active_turbine(void){ - donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01; - donnees_a_envoyer=1; + //donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01; + //donnees_a_envoyer=1; } void i2c_annexe_desactive_turbine(void){ donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;