diff --git a/Balise_VL53L1X.c b/Balise_VL53L1X.c index f7238d8..3443538 100644 --- a/Balise_VL53L1X.c +++ b/Balise_VL53L1X.c @@ -124,7 +124,7 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){ double angle; } cone[NB_CONE]; cone[0].angle = 22 * DEGRE_EN_RADIAN; - cone[0].distance_mm = 800; + cone[0].distance_mm = 1200; cone[1].angle = 50 * DEGRE_EN_RADIAN; cone[1].distance_mm = 580; @@ -152,10 +152,11 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){ if(capteurs_VL53L1X[capteur].donnee_valide){ // Si la distance est plus petite que la distance minimale actuelle // Si la distance est plus petite que le cône en question... - if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale /*&& - capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm*/){ - // On retient cette distance comme étant notre distance minimale - distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm; + if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){ + if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm){ + // On retient cette distance comme étant notre distance minimale + distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm; + } } } // Le capteur est pris en compte au moins dans un cône diff --git a/Geometrie.h b/Geometrie.h index 34feeee..ca506b9 100644 --- a/Geometrie.h +++ b/Geometrie.h @@ -6,6 +6,7 @@ #endif #define DEGRE_EN_RADIAN (M_PI / 180.) +#define DISTANCE_INVALIDE (-1.) struct position_t{ double x_mm, y_mm; diff --git a/Strategie.c b/Strategie.c index 2c8562b..6a86848 100644 --- a/Strategie.c +++ b/Strategie.c @@ -16,6 +16,9 @@ #define DISTANCE_OBSTACLE_CM 50 #define DISTANCE_PAS_OBSTACLE_MM 2000 +// TODO: Peut-être à remetttre en variable locale après +double distance_obstacle; + enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian); enum etat_action_t lance_balles(uint32_t step_ms); @@ -201,6 +204,8 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; enum etat_trajet_t etat_trajet; + double angle_avancement; + static enum { PARCOURS_INIT, PARCOURS_AVANCE, @@ -213,12 +218,13 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint break; case PARCOURS_AVANCE: - if(Balise_VL53L1X_get_min_distance() distance_obstacle:%3.0f\n", distance); + sleep_ms(100); + lettre = getchar_timeout_us(0); }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); diff --git a/Test_strategie.c b/Test_strategie.c index 850f12b..24ea9a1 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -67,6 +67,7 @@ void affichage_test_strategie(){ int test_strategie(){ printf("A - Accoster.\n"); printf("C - Couleur et tirette.\n"); + printf("E - Evitement\n"); printf("H - Homologation.\n"); printf("L - Longer.\n"); printf("P - Panier.\n"); @@ -169,7 +170,6 @@ int test_homologation(){ void affichage_test_evitement(){ while(1){ - printf(">min_dist:%d\n",Balise_VL53L1X_get_min_distance()); for(uint8_t capteur=0; capteur<12; capteur++){ printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur)); } @@ -218,11 +218,11 @@ int test_evitement(){ } Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 1000,0, + 1000,200, 0, 0); // Angles if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){ - etat_strategie = CALAGE_PANIER_1; + } } diff --git a/Trajet.c b/Trajet.c index 7f84424..e51f6c1 100644 --- a/Trajet.c +++ b/Trajet.c @@ -12,10 +12,12 @@ double position_mm; // Position en mm sur la trajectoire double vitesse_mm_s; double vitesse_max_trajet_mm_s=500; double acceleration_mm_ss; +const double acceleration_mm_ss_obstacle = 1500; struct trajectoire_t trajet_trajectoire; struct position_t position_consigne; double distance_obstacle_mm; +const double distance_pas_obstacle = 2000; /// @brief Initialise le module Trajet. A appeler en phase d'initilisation void Trajet_init(){ @@ -112,6 +114,7 @@ struct position_t Trajet_get_consigne(){ /// @return vitesse déterminée en m/s double Trajet_calcul_vitesse(double pas_de_temps_s){ double vitesse_max_contrainte; + double vitesse_max_contrainte_obstacle; double distance_contrainte,distance_contrainte_obstacle; double vitesse; // Calcul de la vitesse avec acceleration @@ -120,13 +123,6 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){ // Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s) // https://poivron-robotique.fr/Consigne-de-vitesse.html distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm; -/* - distance_contrainte_obstacle = Trajet_get_obstacle_mm(); - - if(distance_contrainte > distance_contrainte_obstacle){ - distance_contrainte = distance_contrainte_obstacle; - }*/ - // En cas de dépassement, on veut garder la contrainte, pour l'instant if(distance_contrainte > 0){ vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte); @@ -134,6 +130,15 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){ vitesse_max_contrainte = 0; } + distance_contrainte_obstacle = Trajet_get_obstacle_mm(); + if(distance_contrainte_obstacle != DISTANCE_INVALIDE){ + vitesse_max_contrainte_obstacle = sqrt(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); + if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){ + vitesse_max_contrainte = vitesse_max_contrainte_obstacle; + } + } + + // Selection de la vitesse la plus faible if(vitesse > vitesse_max_contrainte){ vitesse = vitesse_max_contrainte; @@ -151,4 +156,26 @@ double Trajet_get_obstacle_mm(void){ void Trajet_set_obstacle_mm(double distance_mm){ distance_obstacle_mm = distance_mm; +} + + +/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain +/// @return angle en radian. +double Trajet_get_orientation_avance(){ + struct point_xyo_t point, point_suivant; + double avance_abscisse = 0.01; + double angle; + + if(abscisse >= 1){ + return 0; + } + if(abscisse + avance_abscisse >= 1){ + avance_abscisse = 1 - abscisse; + } + + point = Trajectoire_get_point(&trajet_trajectoire, abscisse); + point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse); + + angle = atan2(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x); + return angle; } \ No newline at end of file diff --git a/Trajet.h b/Trajet.h index 450a05f..64f1617 100644 --- a/Trajet.h +++ b/Trajet.h @@ -12,6 +12,7 @@ enum etat_trajet_t{ // Vitesse et acceleration pour une rotation (rad/s et rad/s²) #define TRAJECT_CONFIG_ROTATION_PURE 2, 2 +extern const double distance_pas_obstacle; void Trajet_init(); void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss); @@ -21,3 +22,4 @@ struct position_t Trajet_get_consigne(void); double Trajet_get_obstacle_mm(void); void Trajet_set_obstacle_mm(double distance_mm); void Trajet_stop(double); +double Trajet_get_orientation_avance(void);