From 4d004496db352a10f074a2ce5fda24f2c68ff4b0 Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 5 Dec 2022 21:59:20 +0100 Subject: [PATCH] =?UTF-8?q?Conversion=20vitesses=20X/Y/Angle=20dans=20le?= =?UTF-8?q?=20r=C3=A9f=C3=A9rentiel=20du=20robot=20en=20consigne=20moteur?= =?UTF-8?q?=20:=20OK?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + Commande_vitesse.c | 16 ++++++++ Commande_vitesse.h | 1 + Geometrie_robot.h | 2 + test.c | 92 ++++++++++++++++++++++++++++++++++++++++++++-- 5 files changed, 109 insertions(+), 3 deletions(-) create mode 100644 Commande_vitesse.c create mode 100644 Commande_vitesse.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 21c5825..cdd460b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ add_executable(test test.c APDS_9960.c Asser_Moteurs.c +Commande_vitesse.c QEI.c gyro.c gyro_L3GD20H.c diff --git a/Commande_vitesse.c b/Commande_vitesse.c new file mode 100644 index 0000000..1e30208 --- /dev/null +++ b/Commande_vitesse.c @@ -0,0 +1,16 @@ +#include "Asser_Moteurs.h" +#include "Geometrie_robot.h" + + +void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s){ + double vitesse_roue_a, vitesse_roue_b, vitesse_roue_c; + + vitesse_roue_a = vitesse_x_mm_s / 2.0 - vitesse_y_mm_s * RACINE_DE_3 / 2.0 - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s; + vitesse_roue_b = vitesse_x_mm_s / 2.0 + vitesse_y_mm_s * RACINE_DE_3 / 2.0 - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s; + vitesse_roue_c = -vitesse_x_mm_s - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s; + + AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_roue_a); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_b); + AsserMoteur_setConsigne_mm_s(MOTEUR_C, vitesse_roue_c); + +} \ No newline at end of file diff --git a/Commande_vitesse.h b/Commande_vitesse.h new file mode 100644 index 0000000..a30747b --- /dev/null +++ b/Commande_vitesse.h @@ -0,0 +1 @@ +void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s); \ No newline at end of file diff --git a/Geometrie_robot.h b/Geometrie_robot.h index e69de29..03a31c7 100644 --- a/Geometrie_robot.h +++ b/Geometrie_robot.h @@ -0,0 +1,2 @@ +#define DISTANCE_ROUES_CENTRE_MM 84.25 +#define RACINE_DE_3 1.73205081 \ No newline at end of file diff --git a/test.c b/test.c index a3637e7..bec7cd0 100644 --- a/test.c +++ b/test.c @@ -12,6 +12,8 @@ #include "QEI.h" #include "Asser_Moteurs.h" #include "Localisation.h" +#include "Commande_vitesse.h" +#include "math.h" const uint LED_PIN = 25; const uint LED_PIN_ROUGE = 28; @@ -29,6 +31,9 @@ int test_vitesse_moteur(enum t_moteur moteur); int test_asser_moteur(void); int test_localisation(void); int test_avance(void); +int test_cde_vitesse_rotation(); +int test_cde_vitesse_rectangle(); +int test_cde_vitesse_cercle(); int main() { bi_decl(bi_program_description("This is a test binary.")); @@ -131,6 +136,9 @@ int mode_test(){ printf("B - pour avance (asser_moteur)\n"); printf("C - pour les codeurs\n"); printf("D - pour les codeurs (somme en mm)\n"); + printf("E - Commande en vitesse - rotation pure\n"); + printf("F - Commande en vitesse - carré\n"); + printf("G - Commande en vitesse - cercle\n"); printf("M - pour les moteurs\n"); printf("L - pour la localisation\n"); stdio_flush(); @@ -156,6 +164,21 @@ int mode_test(){ case 'd': while(test_QIE_mm()); break; + + case 'E': + case 'e': + while(test_cde_vitesse_rotation()); + break; + + case 'F': + case 'f': + while(test_cde_vitesse_rectangle()); + break; + + case 'G': + case 'g': + while(test_cde_vitesse_cercle()); + break; case 'M': case 'm': @@ -183,6 +206,69 @@ int mode_test(){ } +int test_cde_vitesse_rotation(){ + int lettre, _step_ms = 1; + double 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); + do{ + QEI_update(); + AsserMoteur_Gestion(_step_ms); + sleep_ms(_step_ms); + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + + return 0; +} + +int test_cde_vitesse_rectangle(){ + int lettre, _step_ms = 1, temps_ms=0; + + printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n"); + do{ + QEI_update(); + AsserMoteur_Gestion(_step_ms); + + if(temps_ms < 5000){ + commande_vitesse(0, 100, 0); + }else if(temps_ms < 7000){ + commande_vitesse(-100, 0, 0); + }else if(temps_ms < 12000){ + commande_vitesse(0, -100, 0); + }else if(temps_ms < 14000){ + commande_vitesse(100, 0, 0); + }else{ + temps_ms = 0; + } + + sleep_ms(_step_ms); + temps_ms += _step_ms; + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + + return 0; +} + +int test_cde_vitesse_cercle(){ + int lettre, _step_ms = 1, temps_ms=0; + uint64_t t_apres, t_avant; + + printf("déplacement en cercle du robot : 100 mm/s\n"); + do{ + QEI_update(); + AsserMoteur_Gestion(_step_ms); + commande_vitesse(cos((double)temps_ms / 1000.) * 200.0, sin((double)temps_ms /1000.) * 200.0, 0); + temps_ms += _step_ms; + sleep_ms(_step_ms); + + + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + + return 0; +} + int test_avance(void){ int lettre; int _step_ms = 1; @@ -216,9 +302,9 @@ int test_asser_moteur(){ int lettre; int _step_ms = 1; printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n"); - AsserMoteur_setConsigne_mm_s(MOTEUR_A, 500); - AsserMoteur_setConsigne_mm_s(MOTEUR_B, 500); - AsserMoteur_setConsigne_mm_s(MOTEUR_C, 500); + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100); + AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100); multicore_launch_core1(test_asser_moteur_printf); do{ QEI_update();