diff --git a/.vscode/settings.json b/.vscode/settings.json index cd98c5f..ff50aa4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,6 +3,9 @@ "timer.h": "c", "localisation.h": "c", "math.h": "c", - "strategie.h": "c" + "strategie.h": "c", + "trajectoire.h": "c", + "trajet.h": "c", + "asser_position.h": "c" } } \ No newline at end of file diff --git a/Balise_VL53L1X.c b/Balise_VL53L1X.c index 99255c7..fd6a5a1 100644 --- a/Balise_VL53L1X.c +++ b/Balise_VL53L1X.c @@ -59,11 +59,11 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio capteur_VL53L1X->donnee_valide=0; } // Si l'obstacle est à l'extérieur du terrain (on prend 50 mm de marge à l'intérieur du terrain, la balise faisant 100 mm) - /*printf("X:%.1f,Y:%.1f\n", position_obstacle.x_mm, position_obstacle.y_mm); + //printf("X:%.1f,Y:%.1f\n", position_obstacle.x_mm, position_obstacle.y_mm); if((position_obstacle.x_mm < 50) || (position_obstacle.y_mm < 50) || (position_obstacle.x_mm > 1950) || (position_obstacle.y_mm > 2950)) { capteur_VL53L1X->donnee_valide=0; - }*/ + } } diff --git a/Holonome2023.c b/Holonome2023.c index a03c477..536b6fc 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -75,7 +75,8 @@ int main() { AsserMoteur_Init(); Localisation_init(); - while(mode_test()); + //while(mode_test()); + test_homologation(); Gyro_Init(); diff --git a/Strategie.c b/Strategie.c index 2ff1f6a..328818e 100644 --- a/Strategie.c +++ b/Strategie.c @@ -12,7 +12,8 @@ #define SEUIL_RECAL_DIST_MM 75 #define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN) -#define DISTANCE_OBSTACLE_CM 35 +#define DISTANCE_OBSTACLE_CM 50 +#define DISTANCE_PAS_OBSTACLE_MM 2000 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); @@ -30,19 +31,19 @@ void Homologation(uint32_t step_ms){ switch(etat_strategie){ case STRATEGIE_INIT: Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN); - etat_strategie = APPROCHE_CERISE_1_A; + etat_strategie = ATTENTE_TIRETTE; + break; + + case ATTENTE_TIRETTE: + if(attente_tirette() == 0){ + etat_strategie = APPROCHE_CERISE_1_A; + } break; case APPROCHE_CERISE_1_A: Trajet_config(250, 500); Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN); - Trajet_debut_trajectoire(trajectoire); - etat_strategie = APPROCHE_CERISE_1_B; - break; - - case APPROCHE_CERISE_1_B: - etat_trajet = Trajet_avance(step_ms/1000.); - if(etat_trajet == TRAJET_TERMINE){ + if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPE_CERISE_1; } break; @@ -105,6 +106,7 @@ void Homologation(uint32_t step_ms){ case STRATEGIE_FIN: Moteur_Stop(); + i2c_annexe_desactive_propulseur(); break; } } @@ -196,7 +198,7 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint case PARCOURS_AVANCE: if(Balise_VL53L1X_get_min_distance() c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm); printf(">c_pos_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian); + printf(">tirette:%ld:%d\n", temps, attente_tirette()); + printf(">etat_strat:%d\n",etat_strategie); /*switch(etat_strategie){ @@ -126,6 +129,7 @@ int test_homologation(){ do{ i2c_gestion(i2c0); i2c_annexe_gestion(); + Balise_VL53L1X_gestion(); // Routines à 1 ms if(temps_ms != Temps_get_temps_ms()){ temps_ms = Temps_get_temps_ms(); diff --git a/Trajet.c b/Trajet.c index e8b2010..ef3ebe9 100644 --- a/Trajet.c +++ b/Trajet.c @@ -15,6 +15,8 @@ double acceleration_mm_ss; struct trajectoire_t trajet_trajectoire; struct position_t position_consigne; +double distance_obstacle_mm; + /// @brief Initialise le module Trajet. A appeler en phase d'initilisation void Trajet_init(){ abscisse = 0; @@ -74,11 +76,20 @@ enum etat_trajet_t Trajet_avance(double pas_de_temps_s){ } +void Trajet_stop(double pas_de_temps_s){ + vitesse_mm_s = 0; + Trajet_avance(0); +} + /// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier /// où les approximations font que l'abscisse peut ne pas atteindre 1. /// @param abscisse : abscisse sur la trajectoire /// @return 1 si le trajet est terminé, 0 sinon int Trajet_terminee(double abscisse){ + if(abscisse >= 0.99 ){ + return 1; + } + /* if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){ if(abscisse >= 1 ){ return 1; @@ -87,7 +98,7 @@ int Trajet_terminee(double abscisse){ if(abscisse >= 0.99 ){ return 1; } - } + }*/ return 0; } @@ -101,7 +112,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 distance_contrainte; + double distance_contrainte,distance_contrainte_obstacle; double vitesse; // Calcul de la vitesse avec acceleration vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s; @@ -109,6 +120,13 @@ 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); @@ -125,3 +143,12 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){ } return vitesse; } + + +double Trajet_get_obstacle_mm(void){ + return distance_obstacle_mm; +} + +void Trajet_set_obstacle_mm(double distance_mm){ + distance_obstacle_mm = distance_mm; +} \ No newline at end of file diff --git a/Trajet.h b/Trajet.h index 65100a1..450a05f 100644 --- a/Trajet.h +++ b/Trajet.h @@ -18,3 +18,6 @@ void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss); void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); enum etat_trajet_t Trajet_avance(double temps_s); 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);