From 90c40c1fdcbf827beb3f382e35836b9f8014c51e Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 1 Dec 2022 22:47:51 +0100 Subject: [PATCH] =?UTF-8?q?Code=20de=20location=20bien=20avanc=C3=A9,=20il?= =?UTF-8?q?=20y=20a=20un=20soucis=20sur=20le=20gain=20des=20QEI=20!?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Asser_Moteurs.c | 3 ++- CMakeLists.txt | 12 +++++----- Localisation.c | 36 ++++++++++++++++++++++++++++ Localisation.h | 8 +++++++ test.c | 63 ++++++++++++++++++++++++++++++++++++++++++++++--- 5 files changed, 112 insertions(+), 10 deletions(-) create mode 100644 Localisation.c create mode 100644 Localisation.h diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index e591d60..5a1c5e1 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -1,5 +1,6 @@ #include "QEI.h" #include "Moteurs.h" +#include "Asser_Moteurs.h" /*** C'est ici que se fait la conversion en mm * ***/ @@ -43,7 +44,7 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ default: break; } - distance = (double) QEI_get(qei) / (double)IMPULSION_PAR_MM; + distance = QEI_get_mm(qei); temps = step_ms / 1000.0; return distance / temps; diff --git a/CMakeLists.txt b/CMakeLists.txt index 0005cf2..21c5825 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,17 +9,17 @@ pico_sdk_init() add_executable(test test.c +APDS_9960.c Asser_Moteurs.c -spi_nb.c +QEI.c gyro.c -Temps.c -Servomoteur.c gyro_L3GD20H.c gyro_ADXRS453.c -QEI.c -APDS_9960.c +Localisation.c Moteurs.c -) +Temps.c +Servomoteur.c +spi_nb.c) pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) diff --git a/Localisation.c b/Localisation.c new file mode 100644 index 0000000..9669ef3 --- /dev/null +++ b/Localisation.c @@ -0,0 +1,36 @@ +#include "Localisation.h" +#include "QEI.h" +#include "math.h" + +#define DISTANCE_ROUES_CENTRE_MM 84.25 +#define RACINE_DE_3 1.73205081 + +struct position_t position; + +void Localisation_init(){ + position.x_mm = 0; + position.y_mm = 0; + position.angle_radian = 0; +} + +void Localisation_gestion(){ + // 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; + + 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; + + position.angle_radian += - ( distance_roue_a_mm + distance_roue_b_mm + distance_roue_c_mm) / (3 * DISTANCE_ROUES_CENTRE_MM); + + // Projection dans le référentiel du robot + 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); + +} + +struct position_t Localisation_get(void){ + return position; +} diff --git a/Localisation.h b/Localisation.h new file mode 100644 index 0000000..0b4f726 --- /dev/null +++ b/Localisation.h @@ -0,0 +1,8 @@ +struct position_t{ + double x_mm, y_mm; + double angle_radian; +}; + +struct position_t Localisation_get(void); +void Localisation_gestion(); +void Localisation_init(); diff --git a/test.c b/test.c index 047fd2a..a3637e7 100644 --- a/test.c +++ b/test.c @@ -11,6 +11,7 @@ #include "Moteurs.h" #include "QEI.h" #include "Asser_Moteurs.h" +#include "Localisation.h" const uint LED_PIN = 25; const uint LED_PIN_ROUGE = 28; @@ -23,8 +24,10 @@ const uint LED_PIN_NE_PAS_UTILISER = 22; int mode_test(); int test_moteurs(); int test_QIE(); +int test_QIE_mm(); int test_vitesse_moteur(enum t_moteur moteur); -int test_asser_moteur(); +int test_asser_moteur(void); +int test_localisation(void); int test_avance(void); int main() { @@ -60,6 +63,7 @@ int main() { Moteur_Init(); QEI_init(); AsserMoteur_Init(); + Localisation_init(); while(mode_test()); @@ -123,10 +127,12 @@ int main() { int mode_test(){ static int iteration = 3; printf("Appuyez sur une touche pour entrer en mode test :\n"); - printf("A - pour asser_moteurs\n"); + printf("A - pour asser_moteurs (rotation)\n"); printf("B - pour avance (asser_moteur)\n"); printf("C - pour les codeurs\n"); + printf("D - pour les codeurs (somme en mm)\n"); printf("M - pour les moteurs\n"); + printf("L - pour la localisation\n"); stdio_flush(); int rep = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); @@ -145,11 +151,22 @@ int mode_test(){ case 'c': while(test_QIE()); break; + + case 'D': + case 'd': + while(test_QIE_mm()); + break; + case 'M': case 'm': /* code */ while(test_moteurs()); break; + case 'L': + case 'l': + /* code */ + while(test_localisation()); + break; case PICO_ERROR_TIMEOUT: iteration--; if(iteration == 0){ @@ -224,7 +241,10 @@ int test_QIE(){ printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n"); do{ QEI_update(); - printf("Codeur A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME)); + printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n", + QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME), + QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME), + QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME)); sleep_ms(100); lettre = getchar_timeout_us(0); @@ -233,6 +253,43 @@ 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; + do{ + QEI_update(); + a_mm += QEI_get_mm(QEI_A_NAME); + b_mm += QEI_get_mm(QEI_B_NAME); + c_mm += QEI_get_mm(QEI_C_NAME); + printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm); + sleep_ms(100); + + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + return 0; + +} + +int test_localisation(){ + int lettre; + struct position_t position; + + printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n"); + do{ + QEI_update(); + Localisation_gestion(); + position = Localisation_get(); + printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654); + sleep_ms(100); + + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + + return 0; + +} + int test_moteurs(){ int lettre_moteur;