diff --git a/Test.c b/Test.c index f837af2..a1f3840 100644 --- a/Test.c +++ b/Test.c @@ -48,7 +48,8 @@ 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_asser_position_avance(void); -int test_asser_position_avance_et_tourne(int); +int test_asser_position_avance_et_tourne(int, int); +int test_transition_gyro_pas_gyro(void); int test_trajectoire(void); int test_i2c_bus(void); void affiche_localisation(void); @@ -83,6 +84,8 @@ int mode_test(){ printf("M - pour les moteurs\n"); printf("N - Fonctions geometrique\n"); printf("O - Analyse obstacle\n"); + printf("P - Asser Position - perturbation\n"); + printf("Q - Asser Position - transition Gyro -> Pas gyro\n"); printf("T - Trajectoire\n"); printf("U - Scan du bus i2c\n"); printf("V - APDS_9960\n"); @@ -136,12 +139,12 @@ int mode_test(){ case 'I': case 'i': - while(test_asser_position_avance_et_tourne(1)); + while(test_asser_position_avance_et_tourne(1, 0)); break; case 'J': case 'j': - while(test_asser_position_avance_et_tourne(0)); + while(test_asser_position_avance_et_tourne(0, 0)); break; case 'K': @@ -169,6 +172,16 @@ int mode_test(){ while(test_angle_balise()); break; + case 'P': + case 'p': + while(test_asser_position_avance_et_tourne(1, 1)); + break; + + case 'Q': + case 'q': + while(test_transition_gyro_pas_gyro()); + break; + case 'T': case 't': while(test_trajectoire()); @@ -915,12 +928,73 @@ int test_trajectoire(){ } + /// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s) /// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans +/// @param propulseur : 1 pour activer le propulseur toutes les secondes /// @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; +int test_transition_gyro_pas_gyro(void){ + int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0; + uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0; + struct position_t position_consigne; + + position_consigne.angle_radian = 0; + position_consigne.x_mm = 0; + position_consigne.y_mm = 0; + + printf("Le robot tourne sur lui-même, transition sans gyro @ t=5s\n"); + + printf("Init gyroscope\n"); + Gyro_Init(); + printf("C'est parti !\n"); + stdio_flush(); + + set_position_avec_gyroscope(1); + 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(get_position_avec_gyroscope()){ + if(temps_ms % _step_ms_gyro == 0){ + Gyro_Read(_step_ms_gyro); + } + } + + if(temps_ms - temps_ms_init > 5000){ + set_position_avec_gyroscope(0); + } + Localisation_gestion(); + AsserMoteur_Gestion(_step_ms); + + position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /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; +} + + +/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s) +/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans +/// @param propulseur : 1 pour activer le propulseur toutes les secondes +/// @return +int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){ + int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0; + uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0; struct position_t position_consigne; position_consigne.angle_radian = 0; @@ -933,6 +1007,9 @@ int test_asser_position_avance_et_tourne(int m_gyro){ Gyro_Init(); printf("C'est parti !\n"); } + if(propulseur){ + i2c_maitre_init(); + } stdio_flush(); set_position_avec_gyroscope(m_gyro); @@ -942,7 +1019,26 @@ int test_asser_position_avance_et_tourne(int m_gyro){ multicore_launch_core1(affiche_localisation); do{ - while(temps_ms == Temps_get_temps_ms()); + + while(temps_ms == Temps_get_temps_ms()){ + if(propulseur){ + i2c_gestion(i2c0); + i2c_annexe_gestion(); + } + } + if(propulseur){ + tempo_prop++; + if(tempo_prop>1000){ + tempo_prop=0; + if(propulseur_on){ + i2c_annexe_active_propulseur(); + }else{ + i2c_annexe_desactive_propulseur(); + } + propulseur_on = !propulseur_on; + } + } + temps_ms = Temps_get_temps_ms(); temps_ms_old = temps_ms; @@ -954,7 +1050,9 @@ int test_asser_position_avance_et_tourne(int m_gyro){ AsserMoteur_Gestion(_step_ms); position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.; - position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.; + if(!propulseur){ + position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.; + } Asser_Position(position_consigne); @@ -1147,8 +1245,8 @@ 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); - + printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654); + printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie()); } } @@ -1222,13 +1320,17 @@ int test_QIE_mm(){ int test_localisation(){ int lettre; + int moteurs = 0; struct position_t position; uint32_t temps_ms; - uint32_t _step_ms_gyro = 2, _step_ms=1; + uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0; uint32_t m_gyro = 0; + int16_t vitesse; + int propulseur=0; printf("A - Sans gyroscope\n"); printf("B - Avec Gyroscope\n"); + printf("C - Avec Gyroscope + moteurs\n"); do{ lettre = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); @@ -1240,6 +1342,10 @@ int test_localisation(){ set_position_avec_gyroscope(0); printf("Sans gyroscope\n"); break; + case 'c': + case 'C': + moteurs = 1; + i2c_maitre_init(); case 'B': case 'b': set_position_avec_gyroscope(1); @@ -1254,11 +1360,13 @@ int test_localisation(){ temps_ms = Temps_get_temps_ms(); multicore_launch_core1(affiche_localisation); - + vitesse = (int16_t) 32766.0; printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n"); do{ + i2c_gestion(i2c0); + i2c_annexe_gestion(); while(temps_ms == Temps_get_temps_ms()); QEI_update(); if(m_gyro){ @@ -1268,6 +1376,23 @@ int test_localisation(){ } Localisation_gestion(); position = Localisation_get(); + if(moteurs){ + tempo_moteur++; + if(tempo_moteur > 1000){ + tempo_moteur = 0; + vitesse = -vitesse; + Moteur_SetVitesse(MOTEUR_A ,vitesse); + Moteur_SetVitesse(MOTEUR_B ,vitesse); + Moteur_SetVitesse(MOTEUR_C ,vitesse); + if(propulseur){ + i2c_annexe_active_propulseur(); + }else{ + i2c_annexe_desactive_propulseur(); + } + propulseur = !propulseur; + } + } + lettre = getchar_timeout_us(0);