From ec67302805be1bd4dfbd43531851d66336d89c72 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 6 May 2023 23:46:00 +0200 Subject: [PATCH] double => float + optimisation du temps de cycle --- Holonome2023.c | 129 +++++++++++++++--------- Monitoring.c | 17 ++++ Monitoring.h | 5 +- Strategie.c | 205 +++++++++++++++++++++++++++++++++----- Strategie.h | 5 +- Strategie_prise_cerises.c | 6 +- Strategie_prise_cerises.h | 2 +- Test.c | 2 +- Test_strategie.c | 7 +- Trajectoire.c | 11 +- Trajectoire.h | 8 +- Trajectoire_bezier.c | 18 ++-- Trajectoire_bezier.h | 2 +- Trajet.c | 12 ++- Trajet.h | 1 + 15 files changed, 328 insertions(+), 102 deletions(-) diff --git a/Holonome2023.c b/Holonome2023.c index 76892ad..0c674cf 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -2,6 +2,7 @@ #include "pico/multicore.h" #include "pico/stdlib.h" #include "hardware/gpio.h" +#include "hardware/i2c.h" #include "hardware/timer.h" #include "pico/binary_info.h" #include "math.h" @@ -10,10 +11,15 @@ #include "gyro.h" #include "Asser_Moteurs.h" #include "Asser_Position.h" +#include "Balise_VL53L1X.h" #include "Commande_vitesse.h" +#include "i2c_annexe.h" +#include "i2c_maitre.h" #include "Localisation.h" +#include "Monitoring.h" #include "Moteurs.h" #include "QEI.h" +#include "Robot_config.h" #include "Servomoteur.h" #include "spi_nb.h" #include "Strategie.h" @@ -41,6 +47,7 @@ int main() { uint32_t temps_ms = 0, temps_ms_old; uint32_t temps_us_debut_cycle; + uint32_t score=0; stdio_init_all(); @@ -75,64 +82,94 @@ int main() { AsserMoteur_Init(); Localisation_init(); - while(mode_test()); - //test_panier(); - //test_homologation(); + //while(mode_test()); + i2c_maitre_init(); + Trajet_init(); - Gyro_Init(); + set_position_avec_gyroscope(1); + if(get_position_avec_gyroscope()){ + Gyro_Init(); + } - + multicore_launch_core1(Monitoring_display); temps_ms = Temps_get_temps_ms(); temps_ms_old = temps_ms; while (1) { - u_int16_t step_ms = 2; - float coef_filtre = 1-0.8; + static enum { + MATCH_ATTENTE_TIRETTE, + MATCH_EN_COURS, + MATCH_ARRET_EN_COURS, + MATCH_TERMINEE + } statu_match=MATCH_ATTENTE_TIRETTE; + uint16_t _step_ms_gyro = 2; + const uint16_t _step_ms = 1; + static uint32_t timer_match_ms=0; + static enum couleur_t couleur; + + // Surveillance du temps d'execution + temps_cycle_check(); + + i2c_gestion(i2c0); + i2c_annexe_gestion(); + Balise_VL53L1X_gestion(); - while(temps_ms == Temps_get_temps_ms()); - temps_ms = Temps_get_temps_ms(); - temps_ms_old = temps_ms; - // Tous les pas de step_ms - if(!(temps_ms % step_ms)){ - temps_us_debut_cycle = time_us_32(); - Gyro_Read(step_ms); - - //gyro_affiche(gyro_get_vitesse(), "Angle :"); - // Filtre - angle_gyro = gyro_get_vitesse(); - if(vitesse_filtre_x == V_INIT){ - vitesse_filtre_x = angle_gyro.rot_x; - vitesse_filtre_y = angle_gyro.rot_y; - vitesse_filtre_z = angle_gyro.rot_z; - }else{ - vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre; - vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre; - vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre; + if(temps_ms != Temps_get_temps_ms()){ + timer_match_ms = timer_match_ms + _step_ms; + temps_ms = Temps_get_temps_ms(); + QEI_update(); + Localisation_gestion(); + AsserMoteur_Gestion(_step_ms); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + if(get_position_avec_gyroscope()){ + Gyro_Read(_step_ms_gyro); + } } - temps_cycle = time_us_32() - temps_us_debut_cycle; - //printf("%#x, %#x\n", (float)temps_ms_old / 1000, vitesse_filtre_z); + switch(statu_match){ + case MATCH_ATTENTE_TIRETTE: + if(lire_couleur() == COULEUR_VERT){ + // Tout vert + i2c_annexe_couleur_balise(0b00011100, 0x0FFF); + }else{ + // Tout bleu + i2c_annexe_couleur_balise(0b00000011, 0x0FFF); + } + if(attente_tirette() == 0){ + statu_match = MATCH_EN_COURS; + couleur = lire_couleur(); + timer_match_ms=0; + i2c_annexe_couleur_balise(0, 0x00); + score=5; + i2c_annexe_envoie_score(score); + } + break; - //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000)); - //gyro_affiche(angle_gyro, "Vitesse (°/s),"); - - } + case MATCH_EN_COURS: + if (timer_match_ms > 98000){ + printf("MATCH_ARRET_EN_COURS\n"); + statu_match = MATCH_ARRET_EN_COURS; + } + Strategie(couleur, _step_ms); + break; + + case MATCH_ARRET_EN_COURS: + commande_vitesse_stop(); + i2c_annexe_active_deguisement(); + if (timer_match_ms > 100000){ + statu_match = MATCH_TERMINEE; + } + + break; - // Toutes les 50 ms - if((Temps_get_temps_ms() % 50) == 0){ - struct t_angle_gyro_float m_gyro; - m_gyro = gyro_get_angle_degres(); - printf("%f, %f, %d\n", (float)temps_ms / 1000, m_gyro.rot_z, temps_cycle); - } - - // Toutes les 500 ms - if((Temps_get_temps_ms() % 500) == 0){ - //gyro_affiche(gyro_get_angle(), "Angle :"); - } - // Toutes les secondes - if((Temps_get_temps_ms() % 500) == 0){ - //gyro_get_temp(); + case MATCH_TERMINEE: + Moteur_Stop(); + while(1); + break; + } } } } diff --git a/Monitoring.c b/Monitoring.c index 5af05cb..162d1c8 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -1,9 +1,12 @@ #include "pico/stdlib.h" #include +#include "Monitoring.h" uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_max=0; int lock=0; +uint32_t debug_var=0; +float debug_varf=0; void temps_cycle_check(){ static uint32_t temps_old; @@ -27,11 +30,17 @@ void temps_cycle_reset(){ temps_cycle_max=0; } +void Monitoring_display(){ + temps_cycle_display(); +} + void temps_cycle_display(){ uint32_t temps; 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(); } @@ -42,3 +51,11 @@ uint32_t temps_cycle_get_min(){ uint32_t temps_cycle_get_max(){ return temps_cycle_max; } + +void set_debug_var(uint32_t variable){ + debug_var = variable; +} + +void set_debug_varf(float variable){ + debug_varf = variable; +} \ No newline at end of file diff --git a/Monitoring.h b/Monitoring.h index 6d81b4d..9cb2968 100644 --- a/Monitoring.h +++ b/Monitoring.h @@ -4,4 +4,7 @@ void temps_cycle_check(); void temps_cycle_reset(); void temps_cycle_display(); uint32_t temps_cycle_get_min(); -uint32_t temps_cycle_get_max(); \ No newline at end of file +uint32_t temps_cycle_get_max(); +void set_debug_var(uint32_t variable); +void set_debug_varf(float variable); +void Monitoring_display(); \ No newline at end of file diff --git a/Strategie.c b/Strategie.c index 78a0872..be39260 100644 --- a/Strategie.c +++ b/Strategie.c @@ -19,10 +19,114 @@ // 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 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); -enum etat_strategie_t etat_strategie=STRATEGIE_INIT; +enum etat_homologation_t etat_homologation=STRATEGIE_INIT; + + +void Strategie(enum couleur_t couleur, uint32_t step_ms){ + + float angle_fin; + float recal_pos_x, recal_pos_y; + enum longer_direction_t longer_direction; + enum etat_action_t etat_action; + enum etat_trajet_t etat_trajet; + + struct trajectoire_t trajectoire; + + static enum etat_strategie_t { + STRATEGIE_INIT, + ALLER_CERISE_BAS, + ATTRAPER_CERISE_BAS, + ALLER_CERISE_HAUT, + ATTRAPER_CERISE_HAUT, + ALLER_CERISE_GAUCHE, + ATTRAPER_CERISE_GAUCHE, + ALLER_CERISE_DROITE, + ATTRAPER_CERISE_DROITE, + ALLER_PANIER, + LANCER_PANIER, + }etat_strategie; + + switch(etat_strategie){ + case STRATEGIE_INIT: + if(couleur == COULEUR_BLEU){ + Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); + }else{ + Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); + } + etat_strategie = ALLER_CERISE_BAS; + break; + + case ALLER_CERISE_BAS: + + if(couleur == COULEUR_BLEU){ + angle_fin = 30. * DEGRE_EN_RADIAN; + }else{ + angle_fin = -150. * DEGRE_EN_RADIAN; + } + + Trajet_config(250, 500); + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 857, 156, + Localisation_get().angle_radian, angle_fin); + + if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ + etat_strategie = ATTRAPER_CERISE_BAS; + } + break; + + case ATTRAPER_CERISE_BAS: + recal_pos_y = RAYON_ROBOT; + if(couleur == COULEUR_BLEU){ + longer_direction = LONGER_VERS_C; + recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM; + }else{ + longer_direction = LONGER_VERS_A; + recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM; + } + + etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = ALLER_PANIER; + } + break; + + case ALLER_PANIER: + Trajet_config(500, 500); + if(couleur == COULEUR_BLEU){ + 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); + }else{ + Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, + 485, Localisation_get().y_mm, + 2000-465, 857, + 2000-465,2830, + -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN); + } + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_strategie = LANCER_PANIER; + } + break; + + case LANCER_PANIER: + if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){ + etat_homologation = STRATEGIE_FIN; + } + break; + + case STRATEGIE_FIN: + i2c_annexe_desactive_propulseur(); + commande_vitesse_stop(); + break; + } +} + void Homologation(uint32_t step_ms){ @@ -32,15 +136,15 @@ void Homologation(uint32_t step_ms){ struct trajectoire_t trajectoire; - switch(etat_strategie){ + switch(etat_homologation){ case STRATEGIE_INIT: Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); - etat_strategie = ATTENTE_TIRETTE; + etat_homologation = ATTENTE_TIRETTE; break; case ATTENTE_TIRETTE: if(attente_tirette() == 0){ - etat_strategie = APPROCHE_CERISE_1_A; + etat_homologation = APPROCHE_CERISE_1_A; } break; @@ -48,14 +152,14 @@ void Homologation(uint32_t step_ms){ 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_strategie = ATTRAPE_CERISE_1; + 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); + etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT); if(etat_action == ACTION_TERMINEE){ - etat_strategie = APPROCHE_PANIER_1; + etat_homologation = APPROCHE_PANIER_1; } break; @@ -68,24 +172,13 @@ void Homologation(uint32_t step_ms){ +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = CALAGE_PANIER_1; + etat_homologation = CALAGE_PANIER_1; } break; - case APPROCHE_PANIER_2: - Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 265,2830, - +120. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN); - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = 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_strategie = RECULE_PANIER; + etat_homologation = RECULE_PANIER; } break; @@ -96,14 +189,14 @@ void Homologation(uint32_t step_ms){ 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = LANCE_DANS_PANIER; + etat_homologation = LANCE_DANS_PANIER; } break; case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms) == ACTION_TERMINEE){ - etat_strategie = STRATEGIE_FIN; + etat_homologation = STRATEGIE_FIN; } break; @@ -114,14 +207,14 @@ void Homologation(uint32_t step_ms){ Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = STRATEGIE_FIN; + 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); + 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_strategie = APPROCHE_PANIER_2; + etat_homologation = APPROCHE_PANIER_2; } break; @@ -134,6 +227,64 @@ void Homologation(uint32_t step_ms){ } +enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){ + static enum { + CALAGE_PANIER_1, + RECULE_PANIER, + LANCE_DANS_PANIER, + }etat_lance_balles_dans_panier; + float recal_pos_x, recal_pos_y; + enum longer_direction_t longer_direction; + float point_x, point_y; + + enum etat_action_t etat_action = ACTION_EN_COURS; + + struct trajectoire_t trajectoire; + + switch(etat_lance_balles_dans_panier){ + case CALAGE_PANIER_1: + if(couleur == COULEUR_BLEU){ + recal_pos_x = RAYON_ROBOT; + longer_direction = LONGER_VERS_A; + }else{ + recal_pos_x = 2000- RAYON_ROBOT; + longer_direction = LONGER_VERS_C; + } + + if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){ + etat_lance_balles_dans_panier = RECULE_PANIER; + } + break; + + case RECULE_PANIER: + Trajet_config(250, 500); + if(couleur == COULEUR_BLEU){ + point_x = 180; + point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; + }else{ + point_x = 1735; + point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; + } + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + point_x, point_y, + 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN); + + if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_lance_balles_dans_panier = LANCE_DANS_PANIER; + } + break; + + case LANCE_DANS_PANIER: + Asser_Position_maintien(); + if(lance_balles(step_ms) == ACTION_TERMINEE){ + etat_action=ACTION_TERMINEE; + } + break; + } + return etat_action; + +} + /// @brief Active le propulseur, ouvre la porte, attend qql secondes. /// @param step_ms : pas de temps. /// @return ACTION_EN_COURS ou ACTION_TERMINEE @@ -185,7 +336,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){ if(temporisation_terminee(&tempo_ms, step_ms)){ i2c_annexe_mi_ferme_porte(); etat_lance_balle = LANCE_TEMPO_PROP_ON; - tempo_ms = 500; + tempo_ms = 750; } break; @@ -237,7 +388,7 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint angle_avancement = Trajet_get_orientation_avance(); distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement); Trajet_set_obstacle_mm(distance_obstacle); - + etat_trajet = Trajet_avance(step_ms/1000.); if(etat_trajet == TRAJET_TERMINE){ Trajet_set_obstacle_mm(DISTANCE_INVALIDE); diff --git a/Strategie.h b/Strategie.h index e9ea404..39c1a26 100644 --- a/Strategie.h +++ b/Strategie.h @@ -25,7 +25,7 @@ enum couleur_t{ COULEUR_INCONNUE, }; -extern enum etat_strategie_t{ +extern enum etat_homologation_t{ STRATEGIE_INIT, ATTENTE_TIRETTE, APPROCHE_CERISE_1_A, @@ -42,7 +42,7 @@ extern enum etat_strategie_t{ APPROCHE_PANIER_3, STRATEGIE_FIN, -}etat_strategie; +}etat_homologation; enum etat_action_t cerise_accostage(void); enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction); @@ -54,6 +54,7 @@ enum couleur_t lire_couleur(void); uint attente_tirette(void); enum etat_action_t lance_balles(uint32_t step_ms); int test_panier(void); +void Strategie(enum couleur_t couleur, uint32_t step_ms); int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms); diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 391cd65..5834e46 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -30,8 +30,11 @@ float vitesse_accostage_mm_s=100; /// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure. /// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout. /// @param longer_direction : direction dans laquelle se trouve la bordure +/// @param step_ms : fréquence d'appel +/// @param pos_recalage_x_mm : Position de recalage contre le support de cerises +/// @param pos_recalage_y_mm : Position de recalage contre la bordure du terrain /// @return ACTION_EN_COURS ou ACTION_TERMINEE -enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm){ +enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm, float pos_recalage_y_mm){ enum etat_action_t etat_action = ACTION_EN_COURS; enum longer_direction_t longer_direction_aspire; static uint32_t tempo_ms = 0; @@ -55,6 +58,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct 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) ){ Localisation_set_x(pos_recalage_x_mm); + Localisation_set_y(pos_recalage_y_mm); etat_attrape = TURBINE_DEMARRAGE; } break; diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index bcbe192..f86b38b 100644 --- a/Strategie_prise_cerises.h +++ b/Strategie_prise_cerises.h @@ -1,2 +1,2 @@ #include "Strategie.h" -enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm); +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); diff --git a/Test.c b/Test.c index 6b0864f..18c0893 100644 --- a/Test.c +++ b/Test.c @@ -1421,7 +1421,7 @@ void affiche_monitoring(){ int test_angle_balise(void){ int lettre; - float distance, angle=0; + float distance, angle=3.1281; i2c_maitre_init(); Balise_VL53L1X_init(); diff --git a/Test_strategie.c b/Test_strategie.c index bf2b57c..228d5d3 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -47,7 +47,10 @@ void affichage_test_strategie(){ printf(">tirette:%ld:%d\n", temps, attente_tirette()); - printf(">etat_strat:%ld:%d\n", temps, etat_strategie); + printf(">etat_strat:%ld:%d\n", temps, etat_homologation); + printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm()); + printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance()); + /*switch(etat_strategie){ case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break; @@ -125,6 +128,7 @@ int test_homologation(){ i2c_maitre_init(); Trajet_init(); + Balise_VL53L1X_init(); //printf("Init gyroscope\n"); set_position_avec_gyroscope(1); if(get_position_avec_gyroscope()){ @@ -191,6 +195,7 @@ void affichage_test_evitement(){ for(uint8_t capteur=0; capteur<12; capteur++){ printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur)); } + printf(">distance_minimale:%f\n",Trajet_get_obstacle_mm()); } } diff --git a/Trajectoire.c b/Trajectoire.c index 2cb921b..5fd8c91 100644 --- a/Trajectoire.c +++ b/Trajectoire.c @@ -3,11 +3,12 @@ #include "Trajectoire_circulaire.h" #include "Trajectoire_droite.h" #include "Trajectoire_rotation.h" +#include "Monitoring.h" #include "math.h" #define NB_MAX_TRAJECTOIRES 5 -#define PRECISION_ABSCISSE 0.001 +#define PRECISION_ABSCISSE 0.001f void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon, @@ -115,7 +116,7 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ /// @brief Renvoie le point d'une trajectoire à partir de son abscisse /// @param abscisse : abscisse sur la trajectoire /// @return point en coordonnées X/Y -struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse){ +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){ struct point_xyo_t point_xyo; switch(trajectoire->type){ case TRAJECTOIRE_DROITE: @@ -149,8 +150,8 @@ float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float /// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire /// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire /// @return nouvelle abscisse -float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm){ - float delta_abscisse, delta_mm, erreur_relative; +float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){ + double delta_abscisse, delta_mm, erreur_relative; if(distance_mm == 0){ return abscisse; @@ -175,7 +176,7 @@ float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, flo return abscisse + delta_abscisse; } -float distance_points(struct point_xy_t point, struct point_xy_t point_old){ +double distance_points(struct point_xy_t point, struct point_xy_t point_old){ return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2)); } diff --git a/Trajectoire.h b/Trajectoire.h index 4fab522..501afc8 100644 --- a/Trajectoire.h +++ b/Trajectoire.h @@ -9,7 +9,7 @@ enum trajectoire_type_t{ }; struct point_xy_t{ - float x, y; + double x, y; }; struct point_xyo_t{ @@ -26,10 +26,10 @@ struct trajectoire_t { }; float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); -struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse); +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse); -float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm); -float distance_points(struct point_xy_t point, struct point_xy_t point_old); +float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm); +double distance_points(struct point_xy_t point, struct point_xy_t point_old); void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon, float orientation_debut_rad, float orientation_fin_rad); void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad); diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c index ec8c3cc..5c4aeab 100644 --- a/Trajectoire_bezier.c +++ b/Trajectoire_bezier.c @@ -19,17 +19,17 @@ void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){ /// @brief Retourne le point sur la trajectoire en fonction de l'abscisse /// @param abscisse : compris entre 0 et 1 -struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse){ +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){ struct point_xy_t point; - point.x = (float) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + - 3 * (float) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) + - 3 * (float) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) + - (float) trajectoire->p4.x * abscisse * abscisse * abscisse; + point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) + + (double) trajectoire->p4.x * abscisse * abscisse * abscisse; - point.y = (float) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + - 3 * (float) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) + - 3 * (float) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) + - (float) trajectoire->p4.y * abscisse * abscisse * abscisse; + point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) + + (double) trajectoire->p4.y * abscisse * abscisse * abscisse; return point; } \ No newline at end of file diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h index 0b19c91..5930684 100644 --- a/Trajectoire_bezier.h +++ b/Trajectoire_bezier.h @@ -2,4 +2,4 @@ void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse); \ No newline at end of file +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c index 1b0ee51..e4dca74 100644 --- a/Trajet.c +++ b/Trajet.c @@ -3,6 +3,7 @@ #include "Trajectoire.h" #include "Trajet.h" #include "Asser_Position.h" +#include "Monitoring.h" float Trajet_calcul_vitesse(float temps_s); int Trajet_terminee(float abscisse); @@ -60,6 +61,7 @@ enum etat_trajet_t Trajet_avance(float pas_de_temps_s){ // Calcul de l'abscisse sur la trajectoire abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm); + set_debug_varf(abscisse); // Obtention du point consigne point = Trajectoire_get_point(&trajet_trajectoire, abscisse); @@ -126,14 +128,14 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){ distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm; // 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); + vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte); }else{ 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); + vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){ vitesse_max_contrainte = vitesse_max_contrainte_obstacle; } @@ -177,6 +179,10 @@ float Trajet_get_orientation_avance(){ 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); + angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x); return angle; +} + +float Trajet_get_abscisse(){ + return abscisse; } \ No newline at end of file diff --git a/Trajet.h b/Trajet.h index 284fc57..53c1a7d 100644 --- a/Trajet.h +++ b/Trajet.h @@ -23,3 +23,4 @@ float Trajet_get_obstacle_mm(void); void Trajet_set_obstacle_mm(float distance_mm); void Trajet_stop(float); float Trajet_get_orientation_avance(void); +float Trajet_get_abscisse();