#include #include #include "pico/multicore.h" #include "Asser_Moteurs.h" #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "gyro.h" #include "Localisation.h" #include "Log.h" #include "Moteurs.h" #include "QEI.h" #include "Trajectoire.h" #include "Trajet.h" #include "Temps.h" #include "Robot_config.h" int test_avance(void); int test_cde_vitesse(void); int test_cde_vitesse_rotation(void); 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_trajectoire(void); int test_aller_retour(void); int test_endurance_aller_retour(void); int test_trajectoire_calibration(void); bool continuous_printf=true; #define TEST_TIMEOUT_US 10000000 // Mode test : renvoie 0 pour quitter le mode test int mode_test_deplacement(){ static int iteration = 2; printf("Appuyez sur une touche pour entrer en mode test :\n"); printf("A - AsserMoteur - pour avancer selon Y\n"); printf("B - AsserMoteur - Commande en vitesse...\n"); printf("C - Trajectoires - sans gyroscope...\n"); printf("D - Aller-retour - avec gyroscope...\n"); printf("E - Aller-retour - Endurance - avec gyroscope...\n"); printf("F - Calibration distance\n"); stdio_flush(); int rep = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); switch (rep) { case 'a': case 'A': while(test_avance()); break; case 'b': case 'B': while(test_cde_vitesse()); break; case 'c': case 'C': while(test_trajectoire()); break; case 'd': case 'D': while(test_aller_retour()); break; case 'e': case 'E': while(test_endurance_aller_retour()); break; case 'f': case 'F': while(test_trajectoire_calibration()); 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; } /// @brief Avance doucement (<10cm/s) selon l'axe X /// @param /// @return int test_avance(void){ int lettre; int _step_ms = 1; AsserMoteur_Init(); AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100); AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100); 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; } int test_cde_vitesse(){ printf("A - Commande en vitesse - rectangle\n"); printf("B - Commande en vitesse - cercle\n"); printf("C - Commande en vitesse - rotation pure\n"); printf("D - Commande en vitesse - rotation par rapport à un point (contacteur longer A)\n"); printf("E - Commande en vitesse - rotation par rapport à un point (contacteur longer C)\n"); int lettre; do{ lettre = getchar_timeout_us(0); stdio_flush(); }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0'); switch(lettre){ case 'a': case 'A': while(test_cde_vitesse_rectangle()); break; case 'b': case 'B': while(test_cde_vitesse_cercle()); break; case 'c': case 'C': while(test_cde_vitesse_rotation()); break; case 'd': case 'D': while(test_cde_rotation_ref_robot(RAYON_ROBOT, 0)); break; case 'e': case 'E': while(test_cde_rotation_ref_robot(RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2)); break; } } int test_cde_vitesse_rotation(){ int lettre, _step_ms = 1; float vitesse =90.0/2 * 3.14159 /180.0; AsserMoteur_Init(); 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); Moteur_Stop(); return 0; } int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){ int lettre, _step_ms = 1; float vitesse =90.0/4 * 3.14159 /180.0; AsserMoteur_Init(); 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); do{ QEI_update(); AsserMoteur_Gestion(_step_ms); sleep_ms(_step_ms); lettre = getchar_timeout_us(0); }while(lettre == PICO_ERROR_TIMEOUT); Moteur_Stop(); return 0; } int test_cde_vitesse_rectangle(){ int lettre, _step_ms = 1, temps_ms=0; AsserMoteur_Init(); 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); Moteur_Stop(); return 0; } int test_cde_vitesse_cercle(){ int lettre, _step_ms = 1, temps_ms=0; AsserMoteur_Init(); printf("déplacement en cercle du robot : 100 mm/s\n"); do{ QEI_update(); AsserMoteur_Gestion(_step_ms); 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); lettre = getchar_timeout_us(0); }while(lettre == PICO_ERROR_TIMEOUT); Moteur_Stop(); return 0; } void test_trajectoire_teleplot(){ struct position_t _position, _consigne; _consigne = Trajet_get_consigne(); while(1){ _consigne = Trajet_get_consigne(); _position = Localisation_get(); uint32_t temps; temps = time_us_32()/1000; if(continuous_printf){ printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); printf(">Position:%f:%f:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps); printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME)); printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); } Log_gestion(); } } /// @brief Lance le robot sur une trajectoire (Bezier, circulaire ou droite) /// Localisation sans Gyroscope /// @return 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, 0, 0); printf("Trajectoire Bezier\n"); break; case 'c': case 'C': Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); printf("Trajectoire circulaire\n"); break; case 'd': case 'D': Trajectoire_droite(&trajectoire, 0, 0, 1000, 0, 0, 0); printf("Trajectoire droite\n"); break; default: return 0; } Trajet_debut_trajectoire(trajectoire); multicore_launch_core1(test_trajectoire_teleplot); do{ // Routines à 1 ms QEI_update(); Localisation_gestion(); if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){ Moteur_SetVitesse(MOTEUR_A, 0); Moteur_SetVitesse(MOTEUR_B, 0); Moteur_SetVitesse(MOTEUR_C, 0); }else{ AsserMoteur_Gestion(_step_ms); } sleep_ms(_step_ms); temps_ms += _step_ms; lettre = getchar_timeout_us(0); lettre = PICO_ERROR_TIMEOUT; }while(lettre == PICO_ERROR_TIMEOUT); return 0; } int test_aller_retour(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; const float corr_angle = 1.145; Trajet_init(); struct trajectoire_t trajectoire; printf("Choix trajectoire :\n"); printf("B - Bezier\n"); printf("C - Circulaire\n"); printf("D - Droite\n"); printf("E - Avance et tourne (ok)\n"); printf("F - Avance et tourne (Nok)\n"); printf("G - Avance (Calibre angle)\n"); printf("R - Rotation pure\n"); do{ lettre = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); }while(lettre == PICO_ERROR_TIMEOUT); switch(lettre){ case 'b': case 'B': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_bezier(&trajectoire, 0, 0, 2500, 0, 250, 1300, 0, 0, 0, 0); printf("Trajectoire de Bézier\n"); break; case 'c': case 'C': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); printf("Trajectoire circulaire\n"); break; case 'd': case 'D': Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0); Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); printf("Trajectoire droite\n"); break; case 'e': case 'E': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_droite(&trajectoire, 0, 0, 0, 1500, 0, M_PI); printf("Trajectoire droite avec rotation (OK)\n"); break; case 'f': case 'F': Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); printf("Trajectoire droite avec rotation (Nok)\n"); break; case 'g': case 'G': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_droite(&trajectoire, 0, 0, 2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)), 0, 0); printf("Trajectoire droite pour calibration angle de départ\n"); break; case 'r': case 'R': Trajectoire_rotation(&trajectoire, 0, 0, 0, 700); trajectoire.orientation_debut_rad = 0; trajectoire.orientation_fin_rad = M_PI; printf("Trajectoire rotation pure\n"); break; default: return 0; } printf("Init gyroscope\n"); Gyro_Init(); printf("C'est parti !\n"); stdio_flush(); set_position_avec_gyroscope(1); Trajet_debut_trajectoire(trajectoire); multicore_launch_core1(test_trajectoire_teleplot); temps_ms = Temps_get_temps_ms(); do{ // Routines à 1 ms while(temps_ms == Temps_get_temps_ms()); temps_ms = Temps_get_temps_ms(); QEI_update(); Localisation_gestion(); // Routine à 2 ms if(temps_ms % _step_ms_gyro == 0){ Gyro_Read(_step_ms_gyro); } if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){ Trajectoire_inverse(&trajectoire); Trajet_debut_trajectoire(trajectoire); }else{ AsserMoteur_Gestion(_step_ms); } lettre = getchar_timeout_us(0); //lettre = PICO_ERROR_TIMEOUT; }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); printf("Lettre : %d; %c\n", lettre, lettre); Moteur_Stop(); multicore_reset_core1(); return 0; } /// @brief Fonction de test d'endurance, notamment pour valider le bon fonctionnement du gyroscope /// sur le long terme et en dynamique. Tente aussi de valider les enregistrements des messages de log. /// Pour l'instant aucune erreur enregistrée, donc pas hyper probant. /// @return int test_endurance_aller_retour(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; const float corr_angle = 1.145; Trajet_init(); Log_init(); struct trajectoire_t trajectoire; printf("Choix trajectoire :\n"); printf("B - Bezier\n"); printf("C - Circulaire\n"); printf("D - Droite\n"); printf("E - Avance et tourne (ok)\n"); printf("F - Avance et tourne (Nok)\n"); printf("G - Avance (Calibre angle)\n"); printf("R - Rotation pure\n"); do{ lettre = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); }while(lettre == PICO_ERROR_TIMEOUT); switch(lettre){ case 'b': case 'B': Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0); Log_message("Trajectoire de Bézier\n", INFO); break; case 'c': case 'C': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); printf("Trajectoire circulaire\n"); break; case 'd': case 'D': Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0); Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); printf("Trajectoire droite\n"); break; case 'e': case 'E': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); printf("Trajectoire droite avec rotation (OK)\n"); break; case 'f': case 'F': Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); printf("Trajectoire droite avec rotation (Nok)\n"); break; case 'g': case 'G': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajectoire_droite(&trajectoire, 0, 0, 2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)), 0, 0); printf("Trajectoire droite pour calibration angle de départ\n"); break; case 'r': case 'R': Trajectoire_rotation(&trajectoire, 0, 0, 0, 700); trajectoire.orientation_debut_rad = 0; trajectoire.orientation_fin_rad = M_PI; printf("Trajectoire droite avec rotation\n"); break; default: return 0; } printf("Init gyroscope\n"); Gyro_Init(); //printf("C'est parti !\n"); stdio_flush(); set_position_avec_gyroscope(1); Trajet_debut_trajectoire(trajectoire); multicore_launch_core1(test_trajectoire_teleplot); temps_ms = Temps_get_temps_ms(); int nb_aller=0; do{ // Routines à 1 ms while(temps_ms == Temps_get_temps_ms()); temps_ms = Temps_get_temps_ms(); QEI_update(); Localisation_gestion(); // Routine à 2 ms if(temps_ms % _step_ms_gyro == 0){ Gyro_Read(_step_ms_gyro); } // Routine à 100 ms if(temps_ms % 100 == 0){ char message[256]; uint32_t temps; struct position_t _position, _consigne; temps = time_us_32()/1000; _consigne = Trajet_get_consigne(); _position = Localisation_get(); /*sprintf(message, ">X:%ld:%.2f\n>Y:%ld:%.2f\n>orientation:%ld:%.2f", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); Log_message(message, TELEPLOT); sprintf(message, ">C_X:%ld:%.2f\n>C_Y:%ld:%.2f\n>C_orientation:%ld:%.2f", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); Log_message(message, TELEPLOT); sprintf(message, ">V_A:%ld:%.2f\n>V_B:%ld:%.2f\n>V_C:%ld:%.2f", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME)); Log_message(message, TELEPLOT); sprintf(message, ">Vc_A:%ld:%.2f\n>Vc_B:%ld:%.2f\n>Vc_C:%ld:%.2f", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); Log_message(message, TELEPLOT);*/ } if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){ char message[256]; nb_aller++; Trajectoire_inverse(&trajectoire); Trajet_debut_trajectoire(trajectoire); if(nb_aller % 2){ sprintf(message, "Aller %d\n", nb_aller / 2 +1); }else{ sprintf(message, "retour %d\n", nb_aller / 2); } Log_message(message, INFO); }else{ AsserMoteur_Gestion(_step_ms); } lettre = getchar_timeout_us(0); //lettre = PICO_ERROR_TIMEOUT; }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); printf("Lettre : %d; %c\n", lettre, lettre); Moteur_Stop(); continuous_printf = 0; printf("Boucle Log\n"); do{ Log_gestion(); lettre = getchar_timeout_us(0); if(lettre == 'L'){ Log_get_full_log(); } }while(lettre != 'q'); printf("Fin boucle Log\n"); return 0; } /// @brief Lance le robot sur une trajectoire (Bezier, circulaire ou droite) /// Localisation avec Gyroscope /// @return int test_trajectoire_calibration(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; Trajet_init(); struct trajectoire_t trajectoire; printf("Choix trajectoire :\n"); printf("A - Rotation sans gyro\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 'a': case 'A': Localisation_set(0, 0, 0); set_position_avec_gyroscope(0); Trajet_config(TRAJECT_CONFIG_ROTATION_PURE); Trajectoire_rotation(&trajectoire, 0, 0, 0, 4 * M_PI); printf("Rotation sans gyro\n"); break; case 'b': case 'B': Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0); printf("Trajectoire Bezier\n"); break; case 'c': case 'C': Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); printf("Trajectoire circulaire\n"); break; case 'd': case 'D': Localisation_set(250, 250, 180. * DEGRE_EN_RADIAN ); set_position_avec_gyroscope(1); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm, Localisation_get().y_mm + 1000, Localisation_get().angle_radian, Localisation_get().angle_radian - (M_PI / 2.)); printf("Trajectoire droite\n"); break; default: return 0; } if(get_position_avec_gyroscope()){ printf("Init gyroscope\n"); Gyro_Init(); } Trajet_debut_trajectoire(trajectoire); multicore_launch_core1(test_trajectoire_teleplot); do{ // Routines à 1 ms QEI_update(); Localisation_gestion(); // Routine à 2 ms if(get_position_avec_gyroscope()){ if(temps_ms % _step_ms_gyro == 0){ Gyro_Read(_step_ms_gyro); } } if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){ Moteur_SetVitesse(MOTEUR_A, 0); Moteur_SetVitesse(MOTEUR_B, 0); Moteur_SetVitesse(MOTEUR_C, 0); }else{ AsserMoteur_Gestion(_step_ms); } sleep_ms(_step_ms); temps_ms += _step_ms; lettre = getchar_timeout_us(0); lettre = PICO_ERROR_TIMEOUT; }while(lettre == PICO_ERROR_TIMEOUT); return 0; }