From 972990d18166d74cdba0a041a9b74577196229d0 Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 19 Dec 2022 19:04:24 +0100 Subject: [PATCH] =?UTF-8?q?Le=20robot=20est=20capable=20d'avancer=20et=20d?= =?UTF-8?q?e=20tourner=20avec=20pr=C3=A9cision.=20-=20Il=20faut=20=C3=AAtr?= =?UTF-8?q?e=20pr=C3=A9cis=20lors=20de=20l'appel=20de=20la=20fonction=20Gy?= =?UTF-8?q?ro=5Fread=20-=20Il=20fallait=20compenser=20le=20gain=20du=20gyr?= =?UTF-8?q?oscope.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Asser_Position.c | 2 +- CMakeLists.txt | 6 +- Commande_vitesse.c | 1 + Localisation.c | 10 +- gyro.c | 2 +- gyro.h | 2 +- gyro_ADXRS453.c | 2 +- test.c | 623 --------------------------------------------- 8 files changed, 18 insertions(+), 630 deletions(-) delete mode 100644 test.c diff --git a/Asser_Position.c b/Asser_Position.c index 8537499..55c43f2 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -3,7 +3,7 @@ #include "math.h" #define GAIN_P_POSITION 100 -#define GAIN_P_ORIENTATION 100 +#define GAIN_P_ORIENTATION 20 /// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot /// C'est à la consigne d'être défini avant pour être atteignable. diff --git a/CMakeLists.txt b/CMakeLists.txt index 42a02d9..5c8e366 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ pico_sdk_init() add_executable(test -test.c +Holonome2023.c APDS_9960.c Asser_Moteurs.c Asser_Position.c @@ -19,7 +19,9 @@ gyro_L3GD20H.c gyro_ADXRS453.c Localisation.c Moteurs.c +Robot_config.c Temps.c +Test.c Trajet.c Trajectoire.c Trajectoire_bezier.c @@ -34,7 +36,7 @@ add_definitions(-DGYRO_ADXRS453) pico_enable_stdio_usb(test 1) pico_enable_stdio_uart(test 1) pico_add_extra_outputs(test) -target_link_libraries(test pico_stdlib hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore) +target_link_libraries(test pico_stdlib hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore) add_custom_target(Flash DEPENDS test diff --git a/Commande_vitesse.c b/Commande_vitesse.c index 953a82b..159ec38 100644 --- a/Commande_vitesse.c +++ b/Commande_vitesse.c @@ -2,6 +2,7 @@ #include "Geometrie_robot.h" /// @brief Commande de la vitesse dans le référentiel du robot +/// Tel que décrit ici : http://poivron-robotique.fr/Robot-holonome-lois-de-commande.html /// @param vitesse_x_mm_s : Vitesse x en mm/s dans le référentiel du robot /// @param vitesse_y_mm_s : Vitesse y en mm/s dans le référentiel du robot /// @param orientation_radian_s : Rotation en radian/s dans le référentiel du robot diff --git a/Localisation.c b/Localisation.c index 643f9fb..db84aa8 100644 --- a/Localisation.c +++ b/Localisation.c @@ -1,7 +1,9 @@ +#include "gyro.h" #include "Localisation.h" #include "QEI.h" #include "math.h" #include "Geometrie_robot.h" +#include "Robot_config.h" struct position_t position; @@ -12,6 +14,7 @@ void Localisation_init(){ } void Localisation_gestion(){ + struct t_angle_gyro_double 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); @@ -21,7 +24,12 @@ void Localisation_gestion(){ 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); + if(get_position_avec_gyroscope()){ + angle_gyro = gyro_get_angle_degres(); + position.angle_radian = angle_gyro.rot_z / 180. * M_PI ; + }else{ + 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); diff --git a/gyro.c b/gyro.c index 4e4a68d..2b1c0ee 100644 --- a/gyro.c +++ b/gyro.c @@ -36,7 +36,7 @@ uint32_t rot_x_zero, rot_y_zero, rot_z_zero; struct t_angle_gyro_double angle_gyro, vitesse_gyro; -struct t_angle_gyro_double gyro_get_angle(void){ +struct t_angle_gyro_double gyro_get_angle_degres(void){ return angle_gyro; } struct t_angle_gyro_double gyro_get_vitesse(void){ diff --git a/gyro.h b/gyro.h index 964646b..cad0519 100644 --- a/gyro.h +++ b/gyro.h @@ -3,6 +3,6 @@ void Gyro_Init(void); void Gyro_Read(uint16_t); void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre); -struct t_angle_gyro_double gyro_get_angle(void); +struct t_angle_gyro_double gyro_get_angle_degres(void); struct t_angle_gyro_double 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 5783f02..80d3943 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -179,7 +179,7 @@ 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; + _vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.0125 / 32.0 * 360. / 357.; // Gain mesuré } diff --git a/test.c b/test.c deleted file mode 100644 index 0bfbd8b..0000000 --- a/test.c +++ /dev/null @@ -1,623 +0,0 @@ -#include -#include "pico/multicore.h" -#include "pico/stdlib.h" -#include "hardware/gpio.h" -#include "pico/binary_info.h" -#include "math.h" - -#include "gyro.h" -#include "Asser_Moteurs.h" -#include "Asser_Position.h" -#include "Commande_vitesse.h" -#include "Localisation.h" -#include "Moteurs.h" -#include "QEI.h" -#include "Servomoteur.h" -#include "spi_nb.h" -#include "Temps.h" -#include "Trajectoire.h" -#include "Trajet.h" - -const uint LED_PIN = 25; -const uint LED_PIN_ROUGE = 28; -const uint LED_PIN_NE_PAS_UTILISER = 22; - - -#define V_INIT -999.0 -#define TEST_TIMEOUT_US 10000000 - -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(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 test_asser_position_avance(); -int test_asser_position_avance_et_tourne(); -int test_trajectoire(); -void affiche_localisation(); - -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; - - uint32_t temps_ms = 0, temps_ms_old; - - stdio_init_all(); - - gpio_init(LED_PIN); - gpio_set_dir(LED_PIN, GPIO_OUT); - gpio_put(LED_PIN, 1); - - gpio_init(LED_PIN_ROUGE); - gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT); - gpio_put(LED_PIN_ROUGE, 1); - - // Il fuat neutraliser cettte broche qui pourrait interférer avec - // la lecture des codeurs. (problème sur la carte électrique)... - gpio_init(LED_PIN_NE_PAS_UTILISER); - gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN); - - sleep_ms(3000); - Servomoteur_Init(); - //puts("Debut"); - //spi_test(); - - //while(1); - Temps_init(); - Moteur_Init(); - QEI_init(); - AsserMoteur_Init(); - Localisation_init(); - - while(mode_test()); - - Gyro_Init(); - - - - 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; - - 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)){ - 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; - } - - //printf("%#x, %#x\n", (double)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),"); - - } - - // Toutes les 50 ms - if((Temps_get_temps_ms() % 50) == 0){ - struct t_angle_gyro_double m_gyro; - m_gyro = gyro_get_angle(); - printf("%f, %f\n", (double)temps_ms / 1000, m_gyro.rot_z); - } - - // 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(); - } - } -} - -// Mode test : renvoie 0 pour quitter le mode test -int mode_test(){ - static int iteration = 2; - printf("Appuyez sur une touche pour entrer en mode test :\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("E - Commande en vitesse - rotation pure\n"); - printf("F - Commande en vitesse - carré\n"); - printf("G - Commande en vitesse - cercle\n"); - printf("H - Asser Position - avance\n"); - printf("I - Asser Position - avance et tourne\n"); - printf("M - pour les moteurs\n"); - printf("L - pour la localisation\n"); - printf("T - Trajectoire\n"); - stdio_flush(); - int rep = getchar_timeout_us(TEST_TIMEOUT_US); - stdio_flush(); - switch (rep) - { - case 'a': - case 'A': - while(test_asser_moteur()); - break; - case 'b': - case 'B': - while(test_avance()); - break; - - case 'C': - case 'c': - while(test_QIE()); - break; - - case 'D': - 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 'H': - case 'h': - while(test_asser_position_avance()); - break; - - case 'I': - case 'i': - while(test_asser_position_avance_et_tourne()); - break; - - case 'M': - case 'm': - /* code */ - while(test_moteurs()); - break; - case 'L': - case 'l': - while(test_localisation()); - break; - - case 'T': - case 't': - while(test_trajectoire()); - break; - - case PICO_ERROR_TIMEOUT: - iteration--; - if(iteration == 0){ - printf("Sortie du mode test\n"); - return 0; - } - - default: - printf("Commande inconnue\n"); - break; - } - return 1; - -} - -void test_trajectoire_printf(){ - struct position_t _position; - while(1){ - _position = Trajet_get_consigne(); - printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180); - } - -} - -int test_trajectoire(){ - int lettre, _step_ms = 1, temps_ms=0; - Trajet_init(); - struct trajectoire_t trajectoire; - printf("Choix trajectoire :\n"); - printf("B - Bezier\n"); - printf("C - Circulaire\n"); - printf("D - Droite\n"); - do{ - lettre = getchar_timeout_us(TEST_TIMEOUT_US); - stdio_flush(); - }while(lettre == PICO_ERROR_TIMEOUT); - switch(lettre){ - case 'b': - case 'B': - Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0); - break; - - case 'c': - case 'C': - Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250); - break; - - case 'd': - case 'D': - Trajectoire_droite(&trajectoire, 0, 0, 0, 500); - break; - - default: return 0; - } - - Trajet_debut_trajectoire(trajectoire); - multicore_launch_core1(test_trajectoire_printf); - do{ - // Routines à 1 ms - QEI_update(); - Localisation_gestion(); - AsserMoteur_Gestion(_step_ms); - - Trajet_avance(_step_ms/1000.); - sleep_ms(_step_ms); - temps_ms += _step_ms; - lettre = getchar_timeout_us(0); - }while(lettre == PICO_ERROR_TIMEOUT); - - return 0; - -} - -int test_asser_position_avance_et_tourne(){ - int lettre, _step_ms = 1, temps_ms=0; - struct position_t position_consigne; - - position_consigne.angle_radian = 0; - position_consigne.x_mm = 0; - position_consigne.y_mm = 0; - - printf("Le robot avance à 100 mm/s\n"); - multicore_launch_core1(affiche_localisation); - do{ - QEI_update(); - Localisation_gestion(); - AsserMoteur_Gestion(_step_ms); - - position_consigne.angle_radian = (double) temps_ms /1000. ; - /* - if(temps_ms < 10000){ - position_consigne.y_mm = (double) temps_ms * 100. / 1000.; - }else if(temps_ms < 10000){ - position_consigne.y_mm = 1000 - (double) temps_ms * 100. / 1000.; - }else{ - temps_ms = 0; - }*/ - - position_consigne.y_mm = (double) temps_ms * 100. / 1000.; - - Asser_Position(position_consigne); - temps_ms += _step_ms; - sleep_ms(_step_ms); - - lettre = getchar_timeout_us(0); - }while(lettre == PICO_ERROR_TIMEOUT); - - return 0; -} - -int test_asser_position_avance(){ - int lettre, _step_ms = 1, temps_ms=0; - struct position_t position; - - position.angle_radian = 0; - position.x_mm = 0; - position.y_mm = 0; - - printf("Le robot avance à 100 mm/s\n"); - do{ - QEI_update(); - Localisation_gestion(); - AsserMoteur_Gestion(_step_ms); - - if(temps_ms < 5000){ - position.x_mm = (double) temps_ms * 100. / 1000.; - }else if(temps_ms < 10000){ - position.x_mm = 1000 - (double) temps_ms * 100. / 1000.; - }else{ - temps_ms = 0; - } - - Asser_Position(position); - temps_ms += _step_ms; - sleep_ms(_step_ms); - - lettre = getchar_timeout_us(0); - }while(lettre == PICO_ERROR_TIMEOUT); - - return 0; -} - -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; - - 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; - AsserMoteur_setConsigne_mm_s(MOTEUR_A, 500); - AsserMoteur_setConsigne_mm_s(MOTEUR_B, -500); - AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0); - - do{ - QEI_update(); - AsserMoteur_Gestion(_step_ms); - sleep_ms(_step_ms); - lettre = getchar_timeout_us(0); - - }while(lettre == PICO_ERROR_TIMEOUT); - Moteur_SetVitesse(MOTEUR_A, 0); - Moteur_SetVitesse(MOTEUR_B, 0); - Moteur_SetVitesse(MOTEUR_C, 0); - return 0; -} - -void affiche_localisation(){ - struct position_t position; - while(1){ - position = Localisation_get(); - printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654); - - } -} - -void test_asser_moteur_printf(){ - int _step_ms = 1; - while(1){ - printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms), - AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms)); - //sleep_ms(5); - } -} - -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, 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(); - AsserMoteur_Gestion(_step_ms); - sleep_ms(_step_ms); - //printf("Vitesse 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("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms), - // AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms)); - lettre = getchar_timeout_us(0); - }while(lettre == PICO_ERROR_TIMEOUT); - Moteur_SetVitesse(MOTEUR_A, 0); - Moteur_SetVitesse(MOTEUR_B, 0); - Moteur_SetVitesse(MOTEUR_C, 0); - multicore_reset_core1(); - return 0; -} - -int test_QIE(){ - int lettre; - printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n"); - do{ - QEI_update(); - 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); - }while(lettre == PICO_ERROR_TIMEOUT); - return 0; - -} - -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; - - printf("Indiquez le moteurs à tester (A, B ou C):\n"); - do{ - lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US); - stdio_flush(); - }while(lettre_moteur == PICO_ERROR_TIMEOUT); - printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur); - - switch (lettre_moteur) - { - case 'A': - case 'a': - while(test_vitesse_moteur(MOTEUR_A)); - break; - - case 'B': - case 'b': - while(test_vitesse_moteur(MOTEUR_B)); - break; - - case 'C': - case 'c': - while(test_vitesse_moteur(MOTEUR_C)); - break; - - case 'Q': - case 'q': - return 0; - break; - - default: - break; - } - - return 1; -} - -int test_vitesse_moteur(enum t_moteur moteur){ - printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n"); - - int vitesse_moteur; - do{ - vitesse_moteur = getchar_timeout_us(TEST_TIMEOUT_US); - stdio_flush(); - }while(vitesse_moteur == PICO_ERROR_TIMEOUT); - - switch (vitesse_moteur) - { - case '0': - case '1': - case '2': - case '3': - case '4': - case '5': - case '6': - case '7': - case '8': - case '9': - printf("Vitesse choisie : %c0%%\n", vitesse_moteur); - Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.); - break; - - case 'A': - case 'a': - printf("Vitesse choisie : 100%%\n"); - Moteur_SetVitesse(moteur, (int16_t) 32766.0); - break; - - case 'b': - case 'B': - printf("Vitesse choisie : -50%%\n"); - Moteur_SetVitesse(moteur, (int16_t) -32766.0/2); - break; - - case 'q': - case 'Q': - return 0; - break; - - default: - break; - } - return 1; -}