From 7c22d99acd450a9b9586809636ddfcb076bda92f Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 28 Apr 2023 23:51:43 +0200 Subject: [PATCH] Float -> Double --- .vscode/c_cpp_properties.json | 4 +-- Asser_Moteurs.c | 20 +++++++------- Asser_Moteurs.h | 6 ++--- Asser_Position.c | 10 +++---- Balise_VL53L1X.c | 46 ++++++++++++++------------------ Balise_VL53L1X.h | 3 ++- CMakeLists.txt | 1 + Commande_vitesse.c | 8 +++--- Commande_vitesse.h | 4 +-- Geometrie.c | 12 +++------ Geometrie.h | 10 +++---- Holonome2023.c | 10 +++---- Localisation.c | 24 ++++++++--------- Localisation.h | 8 +++--- Monitoring.c | 44 ++++++++++++++++++++++++++++++ Monitoring.h | 7 +++++ QEI.c | 4 +-- QEI.h | 2 +- Strategie.c | 8 +++--- Strategie.h | 5 ++-- Strategie_prise_cerises.c | 6 ++--- Strategie_prise_cerises.h | 2 +- Test.c | 49 +++++++++++++++++++++------------- Test_strategie.c | 22 +++++++++++++-- Trajectoire.c | 28 ++++++++++---------- Trajectoire.h | 32 +++++++++++----------- Trajectoire_bezier.c | 22 +++++++-------- Trajectoire_bezier.h | 2 +- Trajectoire_circulaire.c | 8 +++--- Trajectoire_circulaire.h | 2 +- Trajectoire_droite.c | 6 ++--- Trajectoire_droite.h | 2 +- Trajectoire_rotation.c | 2 +- Trajectoire_rotation.h | 2 +- Trajet.c | 50 +++++++++++++++++------------------ Trajet.h | 14 +++++----- gyro.c | 15 ++++++----- gyro.h | 8 +++--- gyro_ADXRS453.c | 8 +++--- gyro_ADXRS453.h | 2 +- gyro_L3GD20H.c | 8 +++--- gyro_L3GD20H.h | 2 +- gyro_data.h | 4 +-- i2c_annexe.c | 2 ++ i2c_maitre.c | 2 +- 45 files changed, 306 insertions(+), 230 deletions(-) create mode 100644 Monitoring.c create mode 100644 Monitoring.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 2686af4..2e38edf 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -2,6 +2,7 @@ "env": { "myDefaultIncludePath": [ "${workspaceFolder}", + "${workspaceFolder}/build", "${env:PICO_SDK_PATH}/src/./common/pico_binary_info/include", "${env:PICO_SDK_PATH}/src/./common/pico_base/include", "${env:PICO_SDK_PATH}/src/./common/pico_time/include", @@ -76,8 +77,7 @@ "${myDefaultIncludePath}" ], "defines": [ - "FOO", - "BAR=100" + "GYRO_ADXRS453" ], "compilerPath": "/usr/bin/arm-none-eabi-gcc", "cStandard": "c11", diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 6e44ea0..4b30cdc 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -17,8 +17,8 @@ #define ASSERMOTEUR_GAIN_P 160 #define ASSERMOTEUR_GAIN_I .80f -double consigne_mm_s[3]; // Consigne de vitesse (en mm/s) -double commande_I[3]; // Terme integral +float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) +float commande_I[3]; // Terme integral void AsserMoteur_Init(){ for(unsigned int i =0; i< 3; i ++){ @@ -30,22 +30,22 @@ void AsserMoteur_Init(){ /// @brief Défini une consigne de vitesse pour le moteur indiqué. /// @param moteur : Moteur à asservir /// @param _consigne_mm_s : consigne de vitesse en mm/s -void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double _consigne_mm_s){ +void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){ consigne_mm_s[moteur] = _consigne_mm_s; } /// @brief Envoie la consigne du moteur /// @param moteur : Moteur à asservir -double AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){ +float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){ return consigne_mm_s[moteur]; } -double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ +float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ enum QEI_name_t qei; - double distance, temps; + float distance, temps; switch (moteur) { case MOTEUR_A: qei = QEI_A_NAME; break; @@ -55,7 +55,7 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ default: break; } distance = QEI_get_mm(qei); - temps = step_ms / 1000.0; + temps = step_ms / 1000.0f; return distance / temps; } @@ -65,9 +65,9 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ void AsserMoteur_Gestion(int step_ms){ // Pour chaque moteur for(uint moteur=MOTEUR_A; moteurangle_ref_terrain = capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian; // Maintien de l'angle entre -PI et PI - while(capteur_VL53L1X->angle_ref_terrain > M_PI){ - capteur_VL53L1X->angle_ref_terrain -= 2* M_PI; - } - while(capteur_VL53L1X->angle_ref_terrain < -M_PI){ - capteur_VL53L1X->angle_ref_terrain += 2* M_PI; - } - + capteur_VL53L1X->angle_ref_terrain = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian); capteur_VL53L1X->distance_lue_cm = distance_capteur_cm; capteur_VL53L1X->distance_obstacle_robot_mm = 10 * (distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM); capteur_VL53L1X->angle_ref_terrain_min = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN); @@ -61,8 +55,8 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio // Positionne l'obstacle sur le terrain struct position_t position_obstacle; //printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain); - position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm; - position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm; + position_obstacle.x_mm = position_robot.x_mm + cosf(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm; + position_obstacle.y_mm = position_robot.y_mm + sinf(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm; capteur_VL53L1X->donnee_valide=1; // Si la distance vaut 0, à invalider @@ -116,12 +110,12 @@ uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){ /// * +/- 90°, à 350 mm /// @param angle_avancement_radiant : angle d'avancement du robot entre -PI et PI /// @return -double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){ +float Balise_VL53L1X_get_distance_obstacle_mm(float angle_avancement_radiant){ const uint8_t NB_CONE=3; uint16_t masque_led=0; struct cone_t{ - double distance_mm; - double angle; + float distance_mm; + float angle, angle_min, angle_max; } cone[NB_CONE]; cone[0].angle = 22 * DEGRE_EN_RADIAN; cone[0].distance_mm = 1200; @@ -132,21 +126,21 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){ cone[2].angle = 90 * DEGRE_EN_RADIAN; cone[2].distance_mm = 350; - double angle_min, angle_max; - double distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM; + for(uint8_t cone_index=0; cone_index M_PI){ angle -= 2* M_PI; } @@ -14,16 +14,12 @@ double Geometrie_get_angle_normalisee(double angle){ return angle; } -/// @brief Indique si un angle est compris entre deux angles +/// @brief Indique si un angle est compris entre deux angles. Les angles doivent être entre -PI et PI. /// @param angle : angle à comparer /// @param angle_min : début de la fourchette /// @param angle_max : fin de la fourchette /// @return 1 si l'angle est compris entre min et max, 0 sinon -unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max){ - angle = Geometrie_get_angle_normalisee(angle); - angle_min = Geometrie_get_angle_normalisee(angle_min); - angle_max = Geometrie_get_angle_normalisee(angle_max); - +unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max){ if(angle_min > angle_max){ // cas où la fourchette comprend -PI. if( (angle > angle_min) || (angle < angle_max)){ @@ -45,7 +41,7 @@ unsigned int Geometrie_compare_angle(double angle, double angle_min, double angl /// @param angle2_min Début de la seconde plage /// @param angle2_max Fin de la seconde plage /// @return 1 si les deux plages s'intersectent, 0 sinon -unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max){ +unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max){ // Pour que les plages s'intersectent, soit : // * angle1_min est compris entre angle2_min et angle2_max // * angle1_max est compris entre angle2_min et angle2_max diff --git a/Geometrie.h b/Geometrie.h index ca506b9..3e38631 100644 --- a/Geometrie.h +++ b/Geometrie.h @@ -9,12 +9,12 @@ #define DISTANCE_INVALIDE (-1.) struct position_t{ - double x_mm, y_mm; - double angle_radian; + float x_mm, y_mm; + float angle_radian; }; -double Geometrie_get_angle_normalisee(double angle); -unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max); -unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max); +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); #endif diff --git a/Holonome2023.c b/Holonome2023.c index c721f1c..76892ad 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -36,8 +36,8 @@ int mode_test(); int main() { bi_decl(bi_program_description("This is a test binary.")); bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED")); - double vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT; - struct t_angle_gyro_double angle_gyro; + float vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT; + struct t_angle_gyro_float angle_gyro; uint32_t temps_ms = 0, temps_ms_old; uint32_t temps_us_debut_cycle; @@ -112,7 +112,7 @@ int main() { } temps_cycle = time_us_32() - temps_us_debut_cycle; - //printf("%#x, %#x\n", (double)temps_ms_old / 1000, vitesse_filtre_z); + //printf("%#x, %#x\n", (float)temps_ms_old / 1000, vitesse_filtre_z); //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000)); //gyro_affiche(angle_gyro, "Vitesse (°/s),"); @@ -121,9 +121,9 @@ int main() { // Toutes les 50 ms if((Temps_get_temps_ms() % 50) == 0){ - struct t_angle_gyro_double m_gyro; + struct t_angle_gyro_float m_gyro; m_gyro = gyro_get_angle_degres(); - printf("%f, %f, %d\n", (double)temps_ms / 1000, m_gyro.rot_z, temps_cycle); + printf("%f, %f, %d\n", (float)temps_ms / 1000, m_gyro.rot_z, temps_cycle); } // Toutes les 500 ms diff --git a/Localisation.c b/Localisation.c index a1bddbc..c1137f4 100644 --- a/Localisation.c +++ b/Localisation.c @@ -13,34 +13,34 @@ void Localisation_init(){ position.angle_radian = 0; } -void Localisation_set(double x_mm, double y_mm, double angle_radian){ +void Localisation_set(float x_mm, float y_mm, float angle_radian){ position.x_mm = x_mm; position.y_mm = y_mm; position.angle_radian = angle_radian; gyro_set_angle_radian(angle_radian); } -void Localisation_set_x(double x_mm){ +void Localisation_set_x(float x_mm){ position.x_mm = x_mm; } -void Localisation_set_y(double y_mm){ +void Localisation_set_y(float y_mm){ position.y_mm = y_mm; } -void Localisation_set_angle(double angle_radian){ +void Localisation_set_angle(float angle_radian){ position.angle_radian = angle_radian; } void Localisation_gestion(){ - struct t_angle_gyro_double angle_gyro; + struct t_angle_gyro_float angle_gyro; // Voir http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html - double distance_roue_a_mm = QEI_get_mm(QEI_A_NAME); - double distance_roue_b_mm = QEI_get_mm(QEI_B_NAME); - double distance_roue_c_mm = QEI_get_mm(QEI_C_NAME); - double delta_x_ref_robot, delta_y_ref_robot; + float distance_roue_a_mm = QEI_get_mm(QEI_A_NAME); + float distance_roue_b_mm = QEI_get_mm(QEI_B_NAME); + float distance_roue_c_mm = QEI_get_mm(QEI_C_NAME); + float delta_x_ref_robot, delta_y_ref_robot; - double old_orientation_radian = position.angle_radian; + float old_orientation_radian = position.angle_radian; delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm) / 3.0; delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm) * RACINE_DE_3 / 3.0; @@ -53,8 +53,8 @@ void Localisation_gestion(){ } // Projection dans le référentiel de la table - position.x_mm += delta_x_ref_robot * cos(position.angle_radian) - delta_y_ref_robot * sin(position.angle_radian); - position.y_mm += delta_x_ref_robot * sin(position.angle_radian) + delta_y_ref_robot * cos(position.angle_radian); + position.x_mm += delta_x_ref_robot * cosf(position.angle_radian) - delta_y_ref_robot * sinf(position.angle_radian); + position.y_mm += delta_x_ref_robot * sinf(position.angle_radian) + delta_y_ref_robot * cosf(position.angle_radian); } diff --git a/Localisation.h b/Localisation.h index 2790e27..12b4716 100644 --- a/Localisation.h +++ b/Localisation.h @@ -4,7 +4,7 @@ struct position_t Localisation_get(void); void Localisation_gestion(); void Localisation_init(); -void Localisation_set(double x_mm, double y_mm, double angle_radian); -void Localisation_set_x(double x_mm); -void Localisation_set_y(double y_mm); -void Localisation_set_angle(double angle_radian); \ No newline at end of file +void Localisation_set(float x_mm, float y_mm, float angle_radian); +void Localisation_set_x(float x_mm); +void Localisation_set_y(float y_mm); +void Localisation_set_angle(float angle_radian); \ No newline at end of file diff --git a/Monitoring.c b/Monitoring.c new file mode 100644 index 0000000..5af05cb --- /dev/null +++ b/Monitoring.c @@ -0,0 +1,44 @@ +#include "pico/stdlib.h" +#include + +uint32_t temps_cycle_min = UINT32_MAX; +uint32_t temps_cycle_max=0; +int lock=0; + +void temps_cycle_check(){ + static uint32_t temps_old; + uint32_t temps, temps_cycle; + + temps = time_us_32(); + temps_cycle = temps - temps_old; + + if(temps_cycle < temps_cycle_min){ + temps_cycle_min = temps_cycle; + } + + if(temps_cycle > temps_cycle_max){ + temps_cycle_max = temps_cycle; + } + temps_old=time_us_32(); +} + +void temps_cycle_reset(){ + temps_cycle_min = UINT32_MAX; + temps_cycle_max=0; +} + +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); + temps_cycle_reset(); +} + +uint32_t temps_cycle_get_min(){ + return temps_cycle_min; +} + +uint32_t temps_cycle_get_max(){ + return temps_cycle_max; +} diff --git a/Monitoring.h b/Monitoring.h new file mode 100644 index 0000000..6d81b4d --- /dev/null +++ b/Monitoring.h @@ -0,0 +1,7 @@ +#include "pico/stdlib.h" + +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 diff --git a/QEI.c b/QEI.c index 5869733..7756595 100644 --- a/QEI.c +++ b/QEI.c @@ -109,6 +109,6 @@ int QEI_get(enum QEI_name_t qei){ /// @brief Renvoi la distance parcourue en mm depuis la lecture précédente /// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) /// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update() -double QEI_get_mm(enum QEI_name_t qei){ - return (double) QEI_get(qei) / (double)IMPULSION_PAR_MM; +float QEI_get_mm(enum QEI_name_t qei){ + return (float) QEI_get(qei) / (float)IMPULSION_PAR_MM; } \ No newline at end of file diff --git a/QEI.h b/QEI.h index 607a09c..56fb9f2 100644 --- a/QEI.h +++ b/QEI.h @@ -14,4 +14,4 @@ extern struct QEI_t QEI_A, QEI_B, QEI_C; void QEI_update(void); void QEI_init(void); int QEI_get(enum QEI_name_t qei); -double QEI_get_mm(enum QEI_name_t qei); \ No newline at end of file +float QEI_get_mm(enum QEI_name_t qei); \ No newline at end of file diff --git a/Strategie.c b/Strategie.c index 68b30a6..78a0872 100644 --- a/Strategie.c +++ b/Strategie.c @@ -17,9 +17,9 @@ #define DISTANCE_PAS_OBSTACLE_MM 2000 // TODO: Peut-être à remetttre en variable locale après -double distance_obstacle; +float 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 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; @@ -194,7 +194,7 @@ 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 -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 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; @@ -220,7 +220,7 @@ 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; + float angle_avancement; static enum { PARCOURS_INIT, diff --git a/Strategie.h b/Strategie.h index cdd332b..e9ea404 100644 --- a/Strategie.h +++ b/Strategie.h @@ -6,7 +6,8 @@ #define COULEUR 15 #define TIRETTE 14 -#define CORR_ANGLE_DEPART_DEGREE (-1.145) +//#define CORR_ANGLE_DEPART_DEGREE (-1.145) +#define CORR_ANGLE_DEPART_DEGREE (0) enum etat_action_t{ ACTION_EN_COURS, @@ -56,7 +57,7 @@ int test_panier(void); int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms); -extern double distance_obstacle; +extern float distance_obstacle; // STRATEGIE_H #endif \ No newline at end of file diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 201a5a4..391cd65 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -25,13 +25,13 @@ void commande_translation_longer_vers_C(); enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction); -double vitesse_accostage_mm_s=100; +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 /// @return ACTION_EN_COURS ou ACTION_TERMINEE -enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, double 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){ enum etat_action_t etat_action = ACTION_EN_COURS; enum longer_direction_t longer_direction_aspire; static uint32_t tempo_ms = 0; @@ -157,7 +157,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire enum etat_action_t cerise_accostage(void){ enum etat_action_t etat_action = ACTION_EN_COURS; - double rotation; + float rotation; static enum { CERISE_AVANCE_DROIT, diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index 0221d08..bcbe192 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, double pos_x_mm); +enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm); diff --git a/Test.c b/Test.c index 528f971..762ba85 100644 --- a/Test.c +++ b/Test.c @@ -19,6 +19,7 @@ #include "i2c_maitre.h" #include "Localisation.h" #include "Moteurs.h" +#include "Monitoring.h" #include "QEI.h" #include "Robot_config.h" #include "Servomoteur.h" @@ -43,7 +44,7 @@ int test_localisation(void); int test_avance(void); int test_cde_vitesse(void); int test_cde_vitesse_rotation(void); -int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm); +int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm); int test_cde_vitesse_rectangle(void); int test_cde_vitesse_cercle(void); int test_asser_position_avance(void); @@ -726,7 +727,7 @@ void test_trajectoire_teleplot(){ int test_aller_retour(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; - const double corr_angle = 1.145; + const float corr_angle = 1.145; Trajet_init(); struct trajectoire_t trajectoire; printf("Choix trajectoire :\n"); @@ -937,8 +938,8 @@ int test_asser_position_avance_et_tourne(int m_gyro){ Localisation_gestion(); AsserMoteur_Gestion(_step_ms); - position_consigne.angle_radian = (double) (temps_ms - temps_ms_init) /1000.; - position_consigne.y_mm = (double) (temps_ms - temps_ms_init) * 100. / 1000.; + position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.; + position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.; Asser_Position(position_consigne); @@ -965,9 +966,9 @@ int test_asser_position_avance(){ AsserMoteur_Gestion(_step_ms); if(temps_ms < 5000){ - position.y_mm = (double) temps_ms * 100. / 1000.; + position.y_mm = (float) temps_ms * 100. / 1000.; }else if(temps_ms < 10000){ - position.y_mm = 1000 - (double) temps_ms * 100. / 1000.; + position.y_mm = 1000 - (float) temps_ms * 100. / 1000.; }else{ temps_ms = 0; } @@ -1026,7 +1027,7 @@ int test_cde_vitesse(){ int test_cde_vitesse_rotation(){ int lettre, _step_ms = 1; - double vitesse =90.0/2 * 3.14159 /180.0; + float vitesse =90.0/2 * 3.14159 /180.0; printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse); commande_vitesse(0, 0, vitesse); @@ -1042,9 +1043,9 @@ int test_cde_vitesse_rotation(){ } -int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm){ +int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){ int lettre, _step_ms = 1; - double vitesse =90.0/4 * 3.14159 /180.0; + float vitesse =90.0/4 * 3.14159 /180.0; printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse); commande_rotation(vitesse, centre_x_mm, centre_y_mm); @@ -1095,7 +1096,7 @@ int test_cde_vitesse_cercle(){ do{ QEI_update(); AsserMoteur_Gestion(_step_ms); - commande_vitesse(cos((double)temps_ms / 1000.) * 200.0, sin((double)temps_ms /1000.) * 200.0, 0); + commande_vitesse(cos((float)temps_ms / 1000.) * 200.0, sin((float)temps_ms /1000.) * 200.0, 0); temps_ms += _step_ms; sleep_ms(_step_ms); @@ -1189,7 +1190,7 @@ int test_QIE(){ int test_QIE_mm(){ int lettre; printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n"); - double a_mm=0, b_mm=0, c_mm=0; + float a_mm=0, b_mm=0, c_mm=0; do{ QEI_update(); a_mm += QEI_get_mm(QEI_A_NAME); @@ -1354,7 +1355,7 @@ int test_vitesse_moteur(enum t_moteur moteur){ int test_geometrie(){ - double angle = 270, angle_min, angle_max; + float angle = 270, angle_min, angle_max; printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN); angle = 180; printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN); @@ -1393,26 +1394,36 @@ int test_geometrie(){ return 0; } +void affiche_monitoring(){ + while(1){ + temps_cycle_display(); + sleep_ms(100); + } +} + int test_angle_balise(void){ - + int lettre; + float distance, angle=0; + i2c_maitre_init(); Balise_VL53L1X_init(); Localisation_set(1000,1500,0); - int lettre; - double distance, angle=0; + + multicore_launch_core1(affiche_monitoring); + do{ + temps_cycle_check(); + i2c_gestion(i2c0); i2c_annexe_gestion(); Balise_VL53L1X_gestion(); distance = Balise_VL53L1X_get_distance_obstacle_mm(angle); - printf(">distance_obstacle:%3.0f\n", distance); - - sleep_ms(100); lettre = getchar_timeout_us(0); - }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); + }while(1); + //}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); return 0; diff --git a/Test_strategie.c b/Test_strategie.c index 24ea9a1..bf2b57c 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -9,6 +9,7 @@ #include "i2c_maitre.h" #include "gyro.h" #include "Localisation.h" +#include "Monitoring.h" #include "QEI.h" #include "Robot_config.h" #include "Strategie.h" @@ -30,6 +31,7 @@ void affichage_test_strategie(){ while(true){ temps = time_us_32()/1000; + temps_cycle_display(); printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A()); printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C()); printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A()); @@ -45,7 +47,7 @@ void affichage_test_strategie(){ printf(">tirette:%ld:%d\n", temps, attente_tirette()); - printf(">etat_strat:%d\n",etat_strategie); + printf(">etat_strat:%ld:%d\n", temps, etat_strategie); /*switch(etat_strategie){ case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break; @@ -118,6 +120,7 @@ int test_strategie(){ int test_homologation(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; + uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0; printf("Homologation\n"); i2c_maitre_init(); @@ -134,16 +137,31 @@ int test_homologation(){ temps_ms = Temps_get_temps_ms(); temps_ms_init = temps_ms; + temps_cycle_old= time_us_32(); do{ + /*temps_cycle[index_temps_cycle++]= time_us_32() - temps_cycle_old; + if(index_temps_cycle >= 10){ + for(int i=0; i<10; i++){ + printf("t_cycle:%d\n", temps_cycle[i]); + } + index_temps_cycle=0; + } + temps_cycle_old=time_us_32();*/ + + temps_cycle_check(); + 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(); QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms); + // Routine à 2 ms if(temps_ms % _step_ms_gyro == 0){ @@ -155,7 +173,7 @@ int test_homologation(){ Homologation(_step_ms); } - lettre = getchar_timeout_us(0); + //lettre = getchar_timeout_us(0); //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); }while(1); printf("STRATEGIE_LOOP_2\n"); diff --git a/Trajectoire.c b/Trajectoire.c index 09fdc0d..2cb921b 100644 --- a/Trajectoire.c +++ b/Trajectoire.c @@ -10,8 +10,8 @@ #define PRECISION_ABSCISSE 0.001 -void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon, - double orientation_debut_rad, double orientation_fin_rad){ +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){ trajectoire->type = TRAJECTOIRE_CIRCULAIRE; trajectoire->p1.x = centre_x; trajectoire->p1.y = centre_y; @@ -23,8 +23,8 @@ void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, trajectoire->orientation_fin_rad = orientation_fin_rad; } -void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, - double orientation_debut_rad, double 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){ trajectoire->type = TRAJECTOIRE_DROITE; trajectoire->p1.x = p1_x; trajectoire->p1.y = p1_y; @@ -35,8 +35,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double trajectoire->orientation_fin_rad = orientation_fin_rad; } -void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y, - double orientation_debut_rad, double orientation_fin_rad){ +void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, + float orientation_debut_rad, float orientation_fin_rad){ trajectoire->type = TRAJECTOIRE_BEZIER; trajectoire->p1.x = p1_x; trajectoire->p1.y = p1_y; @@ -51,7 +51,7 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double trajectoire->orientation_fin_rad = orientation_fin_rad; } -void Trajectoire_rotation(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double orientation_debut_rad, double orientation_fin_rad){ +void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad){ trajectoire->type = TRAJECTOIRE_ROTATION; trajectoire->p1.x = p1_x; trajectoire->p1.y = p1_y; @@ -91,7 +91,7 @@ void Trajectoire_inverse(struct trajectoire_t * trajectoire){ /// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin /// @param trajectoire /// @return Longueur de la trajectoire -double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ +float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ if(trajectoire->longueur > 0){ // La longueur est déjà calculée }else{ @@ -115,7 +115,7 @@ double 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, double abscisse){ +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse){ struct point_xyo_t point_xyo; switch(trajectoire->type){ case TRAJECTOIRE_DROITE: @@ -141,16 +141,16 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou return point_xyo; } -double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse){ - return (double) trajectoire->orientation_debut_rad * (1-abscisse) + (double) trajectoire->orientation_fin_rad * abscisse; +float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){ + return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) trajectoire->orientation_fin_rad * abscisse; } /// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée /// @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 -double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){ - double delta_abscisse, delta_mm, erreur_relative; +float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm){ + float delta_abscisse, delta_mm, erreur_relative; if(distance_mm == 0){ return abscisse; @@ -175,7 +175,7 @@ double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, d return abscisse + delta_abscisse; } -double distance_points(struct point_xy_t point, struct point_xy_t point_old){ +float 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 8791967..4fab522 100644 --- a/Trajectoire.h +++ b/Trajectoire.h @@ -9,33 +9,33 @@ enum trajectoire_type_t{ }; struct point_xy_t{ - double x, y; + float x, y; }; struct point_xyo_t{ struct point_xy_t point_xy; - double orientation; + float orientation; }; struct trajectoire_t { enum trajectoire_type_t type; struct point_xy_t p1, p2, p3, p4; - double orientation_debut_rad, orientation_fin_rad; - double rayon, angle_debut_degre, angle_fin_degre; - double longueur; + float orientation_debut_rad, orientation_fin_rad; + float rayon, angle_debut_degre, angle_fin_degre; + float longueur; }; -double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); -struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); -double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse); -double 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, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, - double rayon, double orientation_debut_rad, double orientation_fin_rad); -void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double orientation_debut_rad, double orientation_fin_rad); -void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y, - double orientation_debut_rad, double orientation_fin_rad); -void Trajectoire_rotation(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double orientation_debut_rad, double orientation_fin_rad); +float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float 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); +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); +void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, + float orientation_debut_rad, float orientation_fin_rad); +void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad); void Trajectoire_inverse(struct trajectoire_t * trajectoire); #endif diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c index 0fb7628..ec8c3cc 100644 --- a/Trajectoire_bezier.c +++ b/Trajectoire_bezier.c @@ -4,12 +4,12 @@ void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){ struct point_xy_t point, point_old; - double nb_pas=500; + float nb_pas=500; trajectoire->longueur=0; point_old = trajectoire->p1; - for(double abscisse=0; abscisse<=1; abscisse += 1./nb_pas){ + for(float abscisse=0; abscisse<=1; abscisse += 1./nb_pas){ point = Trajectoire_bezier_get_point(trajectoire, abscisse); trajectoire->longueur += distance_points(point, point_old); point_old = point; @@ -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, double abscisse){ +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse){ struct point_xy_t point; - 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.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.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; + 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; return point; } \ No newline at end of file diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h index 5930684..0b19c91 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, double abscisse); \ No newline at end of file +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse); \ No newline at end of file diff --git a/Trajectoire_circulaire.c b/Trajectoire_circulaire.c index d190ec7..2ec8726 100644 --- a/Trajectoire_circulaire.c +++ b/Trajectoire_circulaire.c @@ -3,7 +3,7 @@ void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){ - double distance_angulaire; + float distance_angulaire; if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){ distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre; }else{ @@ -14,11 +14,11 @@ void Trajectoire_circulaire_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_circulaire_get_point(struct trajectoire_t * trajectoire, double abscisse){ +struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){ struct point_xy_t point; - double angle_degre; + float angle_degre; - angle_degre = (double) trajectoire->angle_debut_degre * (1-abscisse) + (double) trajectoire->angle_fin_degre * abscisse; + angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse; point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon; point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon; diff --git a/Trajectoire_circulaire.h b/Trajectoire_circulaire.h index 80366f4..6229d88 100644 --- a/Trajectoire_circulaire.h +++ b/Trajectoire_circulaire.h @@ -1,4 +1,4 @@ #include "math.h" void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, double avancement); +struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float avancement); diff --git a/Trajectoire_droite.c b/Trajectoire_droite.c index 74e3529..a2fa2a8 100644 --- a/Trajectoire_droite.c +++ b/Trajectoire_droite.c @@ -7,11 +7,11 @@ void Trajectoire_droite_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_droite_get_point(struct trajectoire_t * trajectoire, double abscisse){ +struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse){ struct point_xy_t point; - point.x = (double) trajectoire->p1.x * (1. - abscisse) + (double) trajectoire->p2.x * abscisse; - point.y = (double) trajectoire->p1.y * (1. - abscisse) + (double) trajectoire->p2.y * abscisse; + point.x = (float) trajectoire->p1.x * (1. - abscisse) + (float) trajectoire->p2.x * abscisse; + point.y = (float) trajectoire->p1.y * (1. - abscisse) + (float) trajectoire->p2.y * abscisse; return point; } \ No newline at end of file diff --git a/Trajectoire_droite.h b/Trajectoire_droite.h index 4937c55..e4ed24c 100644 --- a/Trajectoire_droite.h +++ b/Trajectoire_droite.h @@ -1,4 +1,4 @@ #include "Trajectoire.h" void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, double abscisse); \ No newline at end of file +struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse); \ No newline at end of file diff --git a/Trajectoire_rotation.c b/Trajectoire_rotation.c index dc0d3de..7b8fbb4 100644 --- a/Trajectoire_rotation.c +++ b/Trajectoire_rotation.c @@ -11,6 +11,6 @@ void Trajectoire_rotation_get_longueur(struct trajectoire_t * trajectoire){ } // Le robot reste sur place -struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, double abscisse){ +struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, float abscisse){ return trajectoire->p1; } \ No newline at end of file diff --git a/Trajectoire_rotation.h b/Trajectoire_rotation.h index 0c932bc..08103eb 100644 --- a/Trajectoire_rotation.h +++ b/Trajectoire_rotation.h @@ -1,4 +1,4 @@ #include "Trajectoire.h" void Trajectoire_rotation_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, double abscisse); +struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, float abscisse); diff --git a/Trajet.c b/Trajet.c index 8e5e50c..1b0ee51 100644 --- a/Trajet.c +++ b/Trajet.c @@ -4,20 +4,20 @@ #include "Trajet.h" #include "Asser_Position.h" -double Trajet_calcul_vitesse(double temps_s); -int Trajet_terminee(double abscisse); +float Trajet_calcul_vitesse(float temps_s); +int Trajet_terminee(float abscisse); -double abscisse; // Position entre 0 et 1 sur la trajectoire -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; +float abscisse; // Position entre 0 et 1 sur la trajectoire +float position_mm; // Position en mm sur la trajectoire +float vitesse_mm_s; +float vitesse_max_trajet_mm_s=500; +float acceleration_mm_ss; +const float 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; +float distance_obstacle_mm; +const float distance_pas_obstacle = 2000; /// @brief Initialise le module Trajet. A appeler en phase d'initilisation void Trajet_init(){ @@ -30,7 +30,7 @@ void Trajet_init(){ /// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets /// @param _vitesse_max_trajet_mm_s /// @param _acceleration_mm_ss -void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss){ +void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss){ vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s; acceleration_mm_ss = _acceleration_mm_ss; } @@ -45,8 +45,8 @@ void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){ /// @brief Avance la consigne de position sur la trajectoire /// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde -enum etat_trajet_t Trajet_avance(double pas_de_temps_s){ - double distance_mm; +enum etat_trajet_t Trajet_avance(float pas_de_temps_s){ + float distance_mm; enum etat_trajet_t trajet_etat = TRAJET_EN_COURS; struct point_xyo_t point; struct position_t position; @@ -79,7 +79,7 @@ enum etat_trajet_t Trajet_avance(double pas_de_temps_s){ } -void Trajet_stop(double pas_de_temps_s){ +void Trajet_stop(float pas_de_temps_s){ vitesse_mm_s = 0; Trajet_avance(0); } @@ -88,7 +88,7 @@ void Trajet_stop(double pas_de_temps_s){ /// 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){ +int Trajet_terminee(float abscisse){ /*if(abscisse >= 0.99 ){ return 1; }*/ @@ -113,11 +113,11 @@ struct position_t Trajet_get_consigne(){ /// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire /// @param pas_de_temps_s : temps écoulé en ms /// @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; +float Trajet_calcul_vitesse(float pas_de_temps_s){ + float vitesse_max_contrainte; + float vitesse_max_contrainte_obstacle; + float distance_contrainte,distance_contrainte_obstacle; + float vitesse; // Calcul de la vitesse avec acceleration vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s; @@ -151,21 +151,21 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){ } -double Trajet_get_obstacle_mm(void){ +float Trajet_get_obstacle_mm(void){ return distance_obstacle_mm; } -void Trajet_set_obstacle_mm(double distance_mm){ +void Trajet_set_obstacle_mm(float 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(){ +float Trajet_get_orientation_avance(){ struct point_xyo_t point, point_suivant; - double avance_abscisse = 0.01; - double angle; + float avance_abscisse = 0.01; + float angle; if(abscisse >= 1){ return 0; diff --git a/Trajet.h b/Trajet.h index 64f1617..284fc57 100644 --- a/Trajet.h +++ b/Trajet.h @@ -12,14 +12,14 @@ 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; +extern const float distance_pas_obstacle; void Trajet_init(); -void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss); +void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss); void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); -enum etat_trajet_t Trajet_avance(double temps_s); +enum etat_trajet_t Trajet_avance(float 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); -double Trajet_get_orientation_avance(void); +float Trajet_get_obstacle_mm(void); +void Trajet_set_obstacle_mm(float distance_mm); +void Trajet_stop(float); +float Trajet_get_orientation_avance(void); diff --git a/gyro.c b/gyro.c index 78c6b75..a31d5c0 100644 --- a/gyro.c +++ b/gyro.c @@ -3,6 +3,7 @@ #include "hardware/gpio.h" #include "hardware/spi.h" #include "hardware/structs/spi.h" +#include "Geometrie.h" #include "spi_nb.h" #include "Temps.h" #include "gyro.h" @@ -25,8 +26,8 @@ /// @brief structure d'échange des angles du gyrocope struct t_angle_gyro _vitesse_calibration; struct t_angle_gyro *vitesse_calibration; -struct t_angle_gyro_double _vitesse_angulaire; -struct t_angle_gyro_double *vitesse_angulaire; +struct t_angle_gyro_float _vitesse_angulaire; +struct t_angle_gyro_float *vitesse_angulaire; int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire); void gyro_calibration(void); @@ -35,16 +36,16 @@ uint32_t rot_x_zero, rot_y_zero, rot_z_zero; -struct t_angle_gyro_double angle_gyro, vitesse_gyro; +struct t_angle_gyro_float angle_gyro, vitesse_gyro; -void gyro_set_angle_radian(double angle_radian){ +void gyro_set_angle_radian(float angle_radian){ angle_gyro.rot_z = angle_radian * RADIAN_VERS_DEGRES; } -struct t_angle_gyro_double gyro_get_angle_degres(void){ +struct t_angle_gyro_float gyro_get_angle_degres(void){ return angle_gyro; } -struct t_angle_gyro_double gyro_get_vitesse(void){ +struct t_angle_gyro_float gyro_get_vitesse(void){ return vitesse_gyro; } @@ -121,7 +122,7 @@ int16_t gyro_get_temp(void){ } -void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){ +void gyro_affiche(struct t_angle_gyro_float angle_gyro, char * titre){ if(titre != NULL){ printf("%s ",titre); } diff --git a/gyro.h b/gyro.h index 34fa7ff..cb46cde 100644 --- a/gyro.h +++ b/gyro.h @@ -2,8 +2,8 @@ void Gyro_Init(void); void Gyro_Read(uint16_t); -void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre); -void gyro_set_angle_radian(double angle_radian); -struct t_angle_gyro_double gyro_get_angle_degres(void); -struct t_angle_gyro_double gyro_get_vitesse(void); +void gyro_affiche(struct t_angle_gyro_float angle_gyro, char * titre); +void gyro_set_angle_radian(float angle_radian); +struct t_angle_gyro_float gyro_get_angle_degres(void); +struct t_angle_gyro_float gyro_get_vitesse(void); int16_t gyro_get_temp(void); \ No newline at end of file diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 6252ddb..9e7f67f 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -176,10 +176,10 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro } void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, - struct t_angle_gyro_double * _vitesse_gyro){ - _vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.0125 / 32.0; - _vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.0125 / 32.0; - _vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.0125 / 32.0 * 360. / 357.; // Gain mesuré + struct t_angle_gyro_float * _vitesse_gyro){ + _vitesse_gyro->rot_x = (float)_vitesse_angulaire->rot_x * 0.0125 / 32.0; + _vitesse_gyro->rot_y = (float)_vitesse_angulaire->rot_y * 0.0125 / 32.0; + _vitesse_gyro->rot_z = (float)_vitesse_angulaire->rot_z * 0.0125 / 32.0 * 360. / 357.; // Gain mesuré } diff --git a/gyro_ADXRS453.h b/gyro_ADXRS453.h index f9679a6..a22c99e 100644 --- a/gyro_ADXRS453.h +++ b/gyro_ADXRS453.h @@ -3,4 +3,4 @@ int gyro_init_check(); int gyro_config(); void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy); -void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro); +void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_float * vitesse_gyro); diff --git a/gyro_L3GD20H.c b/gyro_L3GD20H.c index 1f63195..deff14d 100644 --- a/gyro_L3GD20H.c +++ b/gyro_L3GD20H.c @@ -91,10 +91,10 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro } void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, - struct t_angle_gyro_double * _vitesse_gyro){ - _vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.00875 / 32.0; - _vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.00875 / 32.0; - _vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.00875 / 32.0; + struct t_angle_gyro_float * _vitesse_gyro){ + _vitesse_gyro->rot_x = (float)_vitesse_angulaire->rot_x * 0.00875 / 32.0; + _vitesse_gyro->rot_y = (float)_vitesse_angulaire->rot_y * 0.00875 / 32.0; + _vitesse_gyro->rot_z = (float)_vitesse_angulaire->rot_z * 0.00875 / 32.0; } #endif \ No newline at end of file diff --git a/gyro_L3GD20H.h b/gyro_L3GD20H.h index f9679a6..a22c99e 100644 --- a/gyro_L3GD20H.h +++ b/gyro_L3GD20H.h @@ -3,4 +3,4 @@ int gyro_init_check(); int gyro_config(); void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy); -void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro); +void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_float * vitesse_gyro); diff --git a/gyro_data.h b/gyro_data.h index 9d49ec0..4af4b19 100644 --- a/gyro_data.h +++ b/gyro_data.h @@ -3,8 +3,8 @@ #ifndef GYRO_DATA_H #define GYRO_DATA_H -struct t_angle_gyro_double{ - double rot_x, rot_y, rot_z; +struct t_angle_gyro_float{ + float rot_x, rot_y, rot_z; }; struct t_angle_gyro{ diff --git a/i2c_annexe.c b/i2c_annexe.c index 15a36b7..095f1e6 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -73,8 +73,10 @@ 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; + */ } void i2c_annexe_desactive_turbine(void){ donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE; diff --git a/i2c_maitre.c b/i2c_maitre.c index bd20a82..adf2d88 100644 --- a/i2c_maitre.c +++ b/i2c_maitre.c @@ -185,7 +185,7 @@ enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, I2C_nb_a_recevoir = nb_reception; // On appelle la fonction gestion pour gagner du temps. - i2c_gestion(i2c0); + //i2c_gestion(i2c0); m_statu = I2C_STATU_EN_COURS; break;