diff --git a/Holonome2023.c b/Holonome2023.c index abdc460..2b969fb 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/Monitoring.c b/Monitoring.c index 69e15a2..2e090b2 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -2,6 +2,7 @@ #include #include "Monitoring.h" #include "Localisation.h" +#include "Asser_Moteurs.h" uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_max=0; @@ -41,7 +42,12 @@ void Monitoring_display(){ struct position_t position = Localisation_get(); printf(">pos_x:%ld:%f\n", temps, position.x_mm); printf(">pos_y:%ld:%f\n", temps, position.y_mm); - + printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1)); + printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1)); + printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1)); + printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A)); + printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B)); + printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); } } diff --git a/Strategie.c b/Strategie.c index 51728c4..50909a2 100644 --- a/Strategie.c +++ b/Strategie.c @@ -154,12 +154,11 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ } Trajet_config(250, 500); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 156, + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Localisation_get().angle_radian, angle_fin); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_HAUT; - Strategie_set_cerise_dans_robot(6); } break; @@ -175,6 +174,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y); if(etat_action == ACTION_TERMINEE){ + Strategie_set_cerise_dans_robot(6); etat_strategie = ALLER_PANIER; } break; @@ -205,6 +205,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ case ATTRAPER_CERISE_GAUCHE: if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){ + Strategie_set_cerise_dans_robot(10); etat_strategie = ALLER_PANIER; } break; @@ -273,12 +274,16 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step // On va en ligne droite if(Robot_est_dans_zone_cerise_haut(couleur)){ Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 465,2830, + 180,2800, Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); }else if (Robot_est_dans_zone_cerise_gauche()){ - - /// !!! TODO !!! + Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, + Localisation_get().x_mm + 330, Localisation_get().y_mm - 100, + 745, 3000 - 475, + 180, 3000 - 200, + 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, @@ -314,7 +319,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step } -/// Fonction de localisation aproximatives pour la stratégie. +/// Fonction de localisation approximatives pour la stratégie. /// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain /// @return 1 si oui, 0 si non. @@ -343,6 +348,15 @@ int Robot_est_dans_zone_cerise_gauche(){ } +int Robot_est_dans_zone_cerise_droite(){ + if(Localisation_get().x_mm > 2000 - 630 && + Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){ + return 1; + } + return 0; + +} + int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){ if(couleur == COULEUR_BLEU){ if(Localisation_get().y_mm > 2500 && Localisation_get().x_mm < 1000){ diff --git a/i2c_annexe.c b/i2c_annexe.c index a934abc..798e596 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;