From 13d153cbeb2184fe77fcb91f6b1360658c1bbda9 Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 8 May 2023 17:47:39 +0200 Subject: [PATCH] Reprise de la fonction de calage dans un angle. --- Geometrie.c | 16 +++ Geometrie.h | 1 + Monitoring.c | 12 ++- Strategie.c | 208 +++++++++++++++----------------------- Strategie.h | 2 +- Strategie_prise_cerises.c | 26 ++++- Strategie_prise_cerises.h | 6 ++ Test_strategie.c | 2 +- 8 files changed, 142 insertions(+), 131 deletions(-) diff --git a/Geometrie.c b/Geometrie.c index 650b863..124a119 100644 --- a/Geometrie.c +++ b/Geometrie.c @@ -35,6 +35,22 @@ unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_m } } +/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée, +/// donne l'angle consigne pour limiter les rotations inutiles. +/// Tous les angles sont en radian +/// @param angle_depart +/// @param angle_souhaite +/// @return angle_optimal en radian +float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){ + while((angle_depart - angle_souhaite) > M_PI){ + angle_souhaite += 2* M_PI; + } + while((angle_depart - angle_souhaite) < -M_PI){ + angle_souhaite -= 2* M_PI; + } + return angle_souhaite; +} + /// @brief Indique si les deux plages d'angle se recoupent /// @param angle1_min Début de la première plage /// @param angle1_max Fin de la première plage diff --git a/Geometrie.h b/Geometrie.h index 3e38631..7495042 100644 --- a/Geometrie.h +++ b/Geometrie.h @@ -16,5 +16,6 @@ struct position_t{ float Geometrie_get_angle_normalisee(float angle); unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max); unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max); +float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite); #endif diff --git a/Monitoring.c b/Monitoring.c index 3a3f82b..69e15a2 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -1,6 +1,7 @@ #include "pico/stdlib.h" #include #include "Monitoring.h" +#include "Localisation.h" uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_max=0; @@ -32,7 +33,16 @@ void temps_cycle_reset(){ void Monitoring_display(){ while(1){ + uint32_t temps; + temps = time_us_32()/1000; temps_cycle_display(); + printf(">DebugVar:%ld:%d\n", temps, debug_var); + printf(">DebugVarf:%ld:%f\n", temps, debug_varf); + 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); + + } } @@ -41,8 +51,6 @@ void temps_cycle_display(){ temps = time_us_32()/1000; printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min); printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max); - printf(">DebugVar:%ld:%d\n", temps, debug_var); - printf(">DebugVarf:%ld:%f\n", temps, debug_varf); temps_cycle_reset(); } diff --git a/Strategie.c b/Strategie.c index fe1455c..cce6bb5 100644 --- a/Strategie.c +++ b/Strategie.c @@ -25,14 +25,26 @@ struct objectif_t{ } objectifs[NB_OBJECTIFS]; struct objectif_t * objectif_courant; +uint32_t Score_nb_cerises = 0; +uint32_t Score_cerises_dans_robot=0; + +void Score_set_cerise_dans_robot(uint32_t nb_cerises){ + Score_cerises_dans_robot = nb_cerises; +} + +uint32_t Score_get_cerise_dans_robot(void){ + return Score_cerises_dans_robot; +} + // TODO: Peut-être à remetttre en variable locale après float distance_obstacle; -enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms); +enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises); +enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises); enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian); -enum etat_action_t lance_balles(uint32_t step_ms); + int Robot_est_dans_quart_haut_gauche(); int Robot_est_dans_quart_haut_droit(); @@ -124,6 +136,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y); if(etat_action == ACTION_TERMINEE){ + Score_set_cerise_dans_robot(6); etat_strategie = ALLER_PANIER; } break; @@ -144,6 +157,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_HAUT; + Score_set_cerise_dans_robot(6); } break; @@ -170,7 +184,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){ break; case LANCER_PANIER: - if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){ + if(lance_balles_dans_panier(couleur, step_ms, Score_get_cerise_dans_robot())== ACTION_TERMINEE){ objectif_courant->etat = FAIT; etat_strategie = DECISION_STRATEGIE; } @@ -235,7 +249,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step 465,2830, +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); } - }else{ + }else{ // COULEUR_VERT // Si le robot est déjà dans le quart haut droit, // On va en ligne droite if(Robot_est_dans_quart_haut_droit()){ @@ -245,7 +259,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step }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, + 2000-485, Localisation_get().y_mm, 2000-465, 857, 2000-465, 2830, -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN); @@ -279,113 +293,14 @@ int Robot_est_dans_quart_haut_droit(){ return 0; } - -void Homologation(uint32_t step_ms){ - - - enum etat_action_t etat_action; - enum etat_trajet_t etat_trajet; - - struct trajectoire_t trajectoire; - - switch(etat_homologation){ - case STRATEGIE_INIT: - Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); - etat_homologation = ATTENTE_TIRETTE; - break; - - case ATTENTE_TIRETTE: - if(attente_tirette() == 0){ - etat_homologation = APPROCHE_CERISE_1_A; - } - break; - - case APPROCHE_CERISE_1_A: - Trajet_config(250, 500); - Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN); - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ - etat_homologation = ATTRAPE_CERISE_1; - } - break; - - case ATTRAPE_CERISE_1: - etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT); - if(etat_action == ACTION_TERMINEE){ - etat_homologation = APPROCHE_PANIER_1; - } - break; - - case APPROCHE_PANIER_1: - Trajet_config(500, 500); - 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); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_homologation = CALAGE_PANIER_1; - } - break; - - case CALAGE_PANIER_1: - if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ - etat_homologation = RECULE_PANIER; - } - break; - - case RECULE_PANIER: - Trajet_config(250, 500); - Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80, - 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN); - - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_homologation = LANCE_DANS_PANIER; - } - break; - - case LANCE_DANS_PANIER: - Asser_Position_maintien(); - if(lance_balles(step_ms) == ACTION_TERMINEE){ - etat_homologation = STRATEGIE_FIN; - } - break; - - case APPROCHE_CERISE_2: - Trajet_config(250, 500); - Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 830, 3000 - 156, - Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); - - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_homologation = STRATEGIE_FIN; - } - break; - - case ATTRAPPE_CERISE_2: - etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, 3000-RAYON_ROBOT); - if(etat_action == ACTION_TERMINEE){ - etat_homologation = APPROCHE_PANIER_2; - } - break; - - - case STRATEGIE_FIN: - i2c_annexe_desactive_propulseur(); - commande_vitesse_stop(); - break; - } -} - - -enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){ +enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){ static enum { CALAGE_PANIER_1, RECULE_PANIER, LANCE_DANS_PANIER, }etat_lance_balles_dans_panier; float recal_pos_x, recal_pos_y; + float angle_depart, angle_arrivee; enum longer_direction_t longer_direction; float point_x, point_y; @@ -417,9 +332,11 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste point_x = 1735; point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; } + angle_depart = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_RADIAN); + angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, point_y, - 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN); + angle_depart, angle_arrivee); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = LANCE_DANS_PANIER; @@ -428,7 +345,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste case LANCE_DANS_PANIER: Asser_Position_maintien(); - if(lance_balles(step_ms) == ACTION_TERMINEE){ + if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){ etat_action = ACTION_TERMINEE; etat_lance_balles_dans_panier = CALAGE_PANIER_1; } @@ -441,7 +358,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste /// @brief Active le propulseur, ouvre la porte, attend qql secondes. /// @param step_ms : pas de temps. /// @return ACTION_EN_COURS ou ACTION_TERMINEE -enum etat_action_t lance_balles(uint32_t step_ms){ +enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){ enum etat_action_t etat_action = ACTION_EN_COURS; static uint32_t tempo_ms; static uint32_t nb_iteration; @@ -473,7 +390,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){ if(temporisation_terminee(&tempo_ms, step_ms)){ i2c_annexe_ouvre_porte(); nb_iteration++; - if(nb_iteration > 10){ + if(nb_iteration > nb_cerises){ nb_iteration=0; etat_action = ACTION_TERMINEE; etat_lance_balle = LANCE_PROPULSEUR_ON; @@ -498,26 +415,69 @@ enum etat_action_t lance_balles(uint32_t step_ms){ } /// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation +/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){ enum etat_action_t etat_action = ACTION_EN_COURS; struct position_t position; + struct trajectoire_t trajectoire; + static float y_pos_ref; - avance_puis_longe_bordure(longer_direction); - if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) || - ((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){ - etat_action = ACTION_TERMINEE; + static enum { + CONTACT_AVANT, + RECULE_BORDURE, + CONTACT_LATERAL, + RECALE_X + }etat_calage_angle; + + switch(etat_calage_angle){ + case CONTACT_AVANT: + if(cerise_accostage() == ACTION_TERMINEE){ + position = Localisation_get(); + //if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){ + Localisation_set_y(y_mm); + //} + //if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){ + Localisation_set_angle(angle_radian); + //} + y_pos_ref = Localisation_get().y_mm; + etat_calage_angle = RECULE_BORDURE; + } + break; + + case RECULE_BORDURE: + commande_translation_recule_vers_trompe(); + position = Localisation_get(); + if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){ + etat_calage_angle = CONTACT_LATERAL; + } + break; + + case CONTACT_LATERAL: + if(longer_direction == LONGER_VERS_A){ + commande_translation_avance_vers_A(); + if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){ + etat_calage_angle = RECALE_X; + } + }else{ + commande_translation_avance_vers_C(); + if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){ + etat_calage_angle = RECALE_X; + } + } + break; + + case RECALE_X: + etat_action = ACTION_TERMINEE; + commande_vitesse_stop(); + position = Localisation_get(); + //if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){ + Localisation_set_x(x_mm); + //} + etat_calage_angle = CONTACT_AVANT; + break; - position = Localisation_get(); - if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){ - Localisation_set_x(x_mm); - } - if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){ - Localisation_set_y(y_mm); - } - if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){ - Localisation_set_angle(angle_radian); - } } + return etat_action; } diff --git a/Strategie.h b/Strategie.h index 85525fb..2e82a73 100644 --- a/Strategie.h +++ b/Strategie.h @@ -52,7 +52,7 @@ enum etat_action_t depose_cerises(uint32_t step_ms); void Homologation(uint32_t step_ms); enum couleur_t lire_couleur(void); uint attente_tirette(void); -enum etat_action_t lance_balles(uint32_t step_ms); +enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises); int test_panier(void); void Strategie(enum couleur_t couleur, uint32_t step_ms); diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index f9fafc2..02e445d 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -20,8 +20,7 @@ void commande_rotation_contacteur_longer_A(); void commande_rotation_contacteur_longer_C(); -void commande_translation_longer_vers_A(); -void commande_translation_longer_vers_C(); + enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction); @@ -160,6 +159,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire } +/// @brief Viens position le robot contre une bordure ou un support cerise devant lui. enum etat_action_t cerise_accostage(void){ enum etat_action_t etat_action = ACTION_EN_COURS; float rotation; @@ -174,7 +174,7 @@ enum etat_action_t cerise_accostage(void){ switch (etat_accostage) { case CERISE_AVANCE_DROIT: - commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0); + commande_translation_avance_vers_trompe(); if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){ etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A; } @@ -245,6 +245,26 @@ void commande_translation_longer_vers_C(){ commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0); } +void commande_translation_avance_vers_trompe(){ + commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0); +} + +void commande_translation_recule_vers_trompe(){ + commande_vitesse(-vitesse_accostage_mm_s * cos(-M_PI/6), -vitesse_accostage_mm_s * sin(-M_PI/6), 0); +} + +void commande_translation_avance_vers_A(){ + // V_x : V * cos (60°) = V / 2 + // V_y : V * sin (60°) = V * RACINE(3) / 2 + commande_vitesse(vitesse_accostage_mm_s/2., vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0); +} + +void commande_translation_avance_vers_C(){ + // V_x : -V * cos (60°) = -V / 2 + // V_y : -V * sin (60°) = -V * RACINE(3) / 2 + commande_vitesse(-vitesse_accostage_mm_s/2., -vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0); +} + enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){ if(direction ==LONGER_VERS_A){ return LONGER_VERS_C; diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index f86b38b..f343720 100644 --- a/Strategie_prise_cerises.h +++ b/Strategie_prise_cerises.h @@ -1,2 +1,8 @@ #include "Strategie.h" enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm); +void commande_translation_longer_vers_A(); +void commande_translation_longer_vers_C(); +void commande_translation_avance_vers_trompe(); +void commande_translation_recule_vers_trompe(); +void commande_translation_avance_vers_A(); +void commande_translation_avance_vers_C(); \ No newline at end of file diff --git a/Test_strategie.c b/Test_strategie.c index 228d5d3..f7f8544 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -331,7 +331,7 @@ int test_panier(){ break; case TEST_PANIER_LANCE_BALLES: - if(lance_balles(_step_ms) == ACTION_TERMINEE){ + if(lance_balles(_step_ms, 10) == ACTION_TERMINEE){ etat_test_panier=TEST_PANIER_PORTE_OUVERTE; } break;