#include #include "pico/multicore.h" #include "pico/stdlib.h" #include "pico/stdio.h" #include "hardware/gpio.h" #include "hardware/i2c.h" #include "pico/binary_info.h" #include "math.h" #include "Test.h" #include "APDS_9960.h" #include "gyro.h" #include "Asser_Moteurs.h" #include "Asser_Position.h" #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "i2c_annexe.h" #include "i2c_maitre.h" #include "Localisation.h" #include "Moteurs.h" #include "QEI.h" #include "Robot_config.h" #include "Servomoteur.h" #include "spi_nb.h" #include "Temps.h" #include "Trajectoire.h" #include "Trajet.h" #define V_INIT -999.0 #define TEST_TIMEOUT_US 10000000 int test_APDS9960(void); int test_moteurs(void); int test_QIE(void); int test_QIE_mm(void); 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(void); int test_cde_vitesse_rotation(void); int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm); int test_cde_vitesse_rectangle(void); int test_cde_vitesse_cercle(void); int test_asser_position_avance(void); int test_asser_position_avance_et_tourne(int); int test_trajectoire(void); int test_i2c_bus(void); void affiche_localisation(void); int test_i2c_lecture_pico_annex(); int test_i2c_lecture_pico_annex_nb(); int test_i2c_lecture_pico_annex_nb2(); int test_i2c_ecriture_pico_annex_nb(); int test_i2c_ecriture_pico_annex_nb_2(); int test_aller_retour(); void test_trajectoire_teleplot(); // 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...\n"); printf("H - Asser Position - avance\n"); printf("I - Asser Position - avance et tourne (gyro)\n"); printf("J - Asser Position - avance et tourne (sans gyro)\n"); printf("K - Trajets aller retour avec Gyro\n"); printf("L - pour la localisation\n"); printf("M - pour les moteurs\n"); printf("T - Trajectoire\n"); printf("U - Scan du bus i2c\n"); printf("V - APDS_9960\n"); printf("W - Com i2c Pico Annexe\n"); printf("X - Com i2c Pico Annexe - non bloquant\n"); printf("Y - I2C - Turbine & porte\n"); printf("Z - I2C - Turbine & porte + contacteurs - fonctions encapsulees\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()); break; case 'H': case 'h': while(test_asser_position_avance()); break; case 'I': case 'i': while(test_asser_position_avance_et_tourne(1)); break; case 'J': case 'j': while(test_asser_position_avance_et_tourne(0)); break; case 'K': case 'k': while(test_aller_retour()); break; case 'L': case 'l': while(test_localisation()); break; case 'M': case 'm': while(test_moteurs()); break; case 'T': case 't': while(test_trajectoire()); break; case 'U': case 'u': while(test_i2c_bus()); break; case 'V': case 'v': while(test_APDS9960()); break; case 'W': case 'w': while(test_i2c_lecture_pico_annex()); break; case 'X': case 'x': while(test_i2c_lecture_pico_annex_nb2()); break; case 'Y': case 'y': while(test_i2c_ecriture_pico_annex_nb()); break; case 'Z': case 'z': while(test_i2c_ecriture_pico_annex_nb_2()); 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; } int test_continue_test(){ int lettre; printf("q pour quitter, une autre touche pour un nouveau test.\n"); do{ lettre = getchar_timeout_us(0); }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); switch(lettre){ case 'q': case 'Q': return 0; default: return 1; } } bool reserved_addr(uint8_t addr) { return (addr & 0x78) == 0 || (addr & 0x78) == 0x78; } int test_APDS9960(){ int lettre; printf("Initialisation\n"); APDS9960_Init(); printf("Lecture...\n"); /* do{ APDS9960_Lire(); lettre = getchar_timeout_us(0); stdio_flush(); }while(lettre == PICO_ERROR_TIMEOUT);*/ while(1){ APDS9960_Lire(); sleep_ms(100); } return 1; } int test_i2c_lecture_pico_annex(){ i2c_maitre_init(); uint8_t tampon[10]; uint8_t registre=2; uint8_t adresse = 0x17; int ret; ret = i2c_write_blocking(i2c0, adresse,®istre, 1, false); if(ret < 0){ printf("Erreur I2C : %d", ret); return 0; } ret = i2c_read_blocking(i2c_default, adresse, tampon, 10, false); if(ret < 0){ printf("Erreur I2C : %d", ret); }else{ for(int i=0; i<10; i++){ printf("%c", tampon[i]); } printf("\n"); for(int i=0; i<10; i++){ printf("%2x ", tampon[i]); } printf("\n"); } return test_continue_test(); } int test_i2c_lecture_pico_annex_nb(){ i2c_maitre_init(); uint8_t tampon[10]; uint8_t registre=2; uint8_t adresse = 0x17; uint32_t time_i2c[5]; const uint8_t T_MAX_I2C = 10; int ret; time_i2c[0] = time_us_32(); // On charge l'adresse de l'esclave i2c0->hw->enable = 0; i2c0->hw->tar = adresse; i2c0->hw->enable = 1; // On envoie l'adresse du registre à lire // Pas de stop, pas de restart, écriture : 0, i2c0->hw->data_cmd = registre; uint8_t first = false; uint8_t last = false; for(int i=0; ihw->data_cmd = bool_to_bit(first) << I2C_IC_DATA_CMD_RESTART_LSB | bool_to_bit(last) << I2C_IC_DATA_CMD_STOP_LSB | I2C_IC_DATA_CMD_CMD_BITS; // -> 1 for read } time_i2c[1] = time_us_32() - time_i2c[0] ; // On attend la fin de la transaction i2c while(i2c0->hw->status & I2C_IC_STATUS_MST_ACTIVITY_BITS); time_i2c[2] = time_us_32() - time_i2c[0] ; // On lit le tampon I2C // uint8_t * dst; // dst = tampon; for(int i=0; ihw->tx_abrt_source){ printf("Erreur I2C: Abort : %4x\n", i2c0->hw->tx_abrt_source); } //On lit la donnée tampon[i] = (uint8_t) i2c0->hw->data_cmd; } time_i2c[3] = time_us_32() - time_i2c[0] ; // Affichage for(int i=0; i Ferme porte\n"); break; case 'O': case 'o': commande = commande & 0xFD; // 0b1111 1101 printf("=> Ouvre porte\n"); break; case 't': case 'T': commande = commande | 0x01; // 0b0000 0001 printf("=> Active turbine\n"); break; case 'u': case 'U': commande = commande & 0xFE; // 0b1111 1110 printf("=> Arrete turbine\n"); break; case 'p': case 'P': commande = commande | 0x04; // 0b0000 0100 printf("=> Active propulseur\n"); break; case 'm': case 'M': commande = commande & 0xFB; // 0b1111 1011 printf("=> Arrete propulseur\n"); break; case 'q': case 'Q': return 0; break; } tampon[0] = 54; tampon[1] = commande; time_i2c[0] = time_us_32(); time_i2c[2] = 0; while(retour_i2c == I2C_EN_COURS){ time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution i2c_gestion(i2c0); retour_i2c = i2c_ecrire_registre_nb(adresse, registre, tampon, T_I2C_ENVOI); time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution sleep_us(100); // Attente, ou le reste du code } time_i2c[3] = time_us_32() - time_i2c[0]; printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]); return 1; } /// @brief Test les fonctions définies dans I2C_Annexe /// @return 1 pour continuer le test, 0 pour arrêter le test int test_i2c_ecriture_pico_annex_nb_2(){ i2c_maitre_init(); uint32_t time_i2c[5]; const uint8_t T_I2C_ENVOI = 2; static uint8_t commande=0; enum i2c_resultat_t retour_i2c = I2C_EN_COURS; printf("F - Ferme porte\n"); printf("O - Ouvre porte\n"); printf("T - Turbine On\n"); printf("U - Turbine Off\n"); printf("P - Propulseur On\n"); printf("M - Propulseur Off\n"); int lettre; int continue_test=1; time_i2c[0] = time_us_32(); time_i2c[2] = 0; while(continue_test){ lettre = getchar_timeout_us(0); if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){ printf("lettre !\n"); switch(lettre){ case 'F': case 'f': i2c_annexe_ferme_porte(); printf("=> Ferme porte\n"); break; case 'O': case 'o': i2c_annexe_ouvre_porte(); printf("=> Ouvre porte\n"); break; case 't': case 'T': i2c_annexe_active_turbine(); printf("=> Active turbine\n"); break; case 'u': case 'U': i2c_annexe_desactive_turbine(); printf("=> Arrete turbine\n"); break; case 'm': case 'M': i2c_annexe_desactive_propulseur(); printf("=> Arrete propulseur\n"); break; case 'p': case 'P': i2c_annexe_active_propulseur(); printf("=> Active propulseur\n"); break; case 'q': case 'Q': continue_test=0; printf("Quitte\n"); break; default: printf("lettre non reconnue: %d %c\n", lettre, lettre); } } time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution i2c_gestion(i2c0); i2c_annexe_gestion(); printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A()); printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C()); printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A()); printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C()); time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution sleep_us(100); // Attente, ou le reste du code } time_i2c[3] = time_us_32() - time_i2c[0]; printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]); return test_continue_test(); } int test_i2c_bus(){ // Adresse I2C : 0b0100 000 R/W // Lecture des broches sur les registres 0 et 1 // Registre 2 et 3 : valeur des broches en sorties // Registre 4 et 5 : INversion de polarité // Registre 6 et 7 : Configuration entrée (1) ou sortie (0) uint8_t reception[8]; uint8_t emission[8]; //uint8_t adresse = 0b0100000; uint8_t adresse = 0x20; int statu; int lettre; emission[0]=6; // Registre à lire i2c_maitre_init(); // Scan bus I2C - cf SDK printf("\nI2C Bus Scan\n"); printf(" 0 1 2 3 4 5 6 7 8 9 A B C D E F\n"); for (int addr = 0; addr < (1 << 7); ++addr) { if (addr % 16 == 0) { printf("%02x ", addr); } int ret; uint8_t rxdata=0x55; if (reserved_addr(addr)) ret = PICO_ERROR_GENERIC; else ret = i2c_read_blocking(i2c_default, addr, &rxdata, 1, false); printf(ret < 0 ? "." : "@"); printf(addr % 16 == 15 ? "\n" : " "); } printf("Done.\n"); return 0; do{ statu = i2c_write_blocking (i2c0, adresse, emission, 1, 0); if(statu == PICO_ERROR_GENERIC){ printf("Emission : Address not acknowledged, no device present.\n"); return 0; }else{ printf("Emission : Ok\n"); } statu = i2c_read_blocking(i2c0, adresse, reception, 2, 0); if(statu == PICO_ERROR_GENERIC){ printf("Reception : Address not acknowledged, no device present.\n"); return 0; }else{ printf("Recetion : Ok\n"); } printf("%2.x%2.x\n",reception[0], reception[1]); lettre = getchar_timeout_us(0); stdio_flush(); }while(lettre == PICO_ERROR_TIMEOUT); return 0; } 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); } } 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; 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)); } } int test_aller_retour(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; 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("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); printf("Trajectoire de Bézier\n"); break; case 'c': case 'C': Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); Trajectoire_circulaire(&trajectoire, 0, 700, -90, 90, 350, 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 '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(); 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; } 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, 0, 700, 0, 0); printf("Trajectoire droite\n"); break; default: return 0; } sleep_ms(3000); 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; } /// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s) /// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans /// @return int test_asser_position_avance_et_tourne(int m_gyro){ int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2; uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old; 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"); if(m_gyro){ printf("Init gyroscope\n"); Gyro_Init(); printf("C'est parti !\n"); } stdio_flush(); set_position_avec_gyroscope(m_gyro); temps_ms = Temps_get_temps_ms(); temps_ms_old = temps_ms; temps_ms_init = temps_ms; multicore_launch_core1(affiche_localisation); do{ while(temps_ms == Temps_get_temps_ms()); temps_ms = Temps_get_temps_ms(); temps_ms_old = temps_ms; QEI_update(); if(temps_ms % _step_ms_gyro == 0){ Gyro_Read(_step_ms_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.; Asser_Position(position_consigne); lettre = getchar_timeout_us(0); }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); printf("lettre : %c, %d\n", lettre, lettre); 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.y_mm = (double) temps_ms * 100. / 1000.; }else if(temps_ms < 10000){ position.y_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(){ 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; 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); Moteur_Stop(); return 0; } int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm){ int lettre, _step_ms = 1; double 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); 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; 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; 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); Moteur_Stop(); return 0; } int test_avance(void){ int lettre; int _step_ms = 1; 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; } 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; uint32_t temps_ms; uint32_t _step_ms_gyro = 2, _step_ms=1; uint32_t m_gyro = 0; printf("A - Sans gyroscope\n"); printf("B - Avec Gyroscope\n"); do{ lettre = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); }while(lettre == PICO_ERROR_TIMEOUT); switch(lettre){ case 'A': case 'a': set_position_avec_gyroscope(0); printf("Sans gyroscope\n"); break; case 'B': case 'b': set_position_avec_gyroscope(1); printf("Avec gyroscope, initialisation...\n"); m_gyro=1; Gyro_Init(); break; default: return 0; } temps_ms = Temps_get_temps_ms(); multicore_launch_core1(affiche_localisation); printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n"); do{ while(temps_ms == Temps_get_temps_ms()); QEI_update(); if(m_gyro){ if(temps_ms % _step_ms_gyro == 0){ Gyro_Read(_step_ms_gyro); } } Localisation_gestion(); position = Localisation_get(); lettre = getchar_timeout_us(0); temps_ms += _step_ms; }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); multicore_reset_core1(); 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(0); 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; }