diff --git a/Geometrie_robot.h b/Geometrie_robot.h index 44b6076..ef75295 100644 --- a/Geometrie_robot.h +++ b/Geometrie_robot.h @@ -1,4 +1,4 @@ -#define DISTANCE_ROUES_CENTRE_MM 84.25 +#define DISTANCE_ROUES_CENTRE_MM 112 // Distance entre le centre du robot et un angle du robot #define RAYON_ROBOT 125 diff --git a/QEI.c b/QEI.c index fb6a76c..f09bd6b 100644 --- a/QEI.c +++ b/QEI.c @@ -86,8 +86,8 @@ void QEI_update(void){ } /// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente -/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation. -/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique +/// Les signes sont inversés (sauf A) car le réducteur inverse le sens de rotation. +/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électronique /// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) /// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update() int QEI_get(enum QEI_name_t qei){ diff --git a/Readme.md b/Readme.md index aa758e3..91bb453 100644 --- a/Readme.md +++ b/Readme.md @@ -77,4 +77,38 @@ Le fichier [i2c_maitre.c](i2c_maitre.c) propose une implémentation non-bloquant - La fonction *i2c_gestion* doit être appelée régulièrement. - La fonction *i2c_transmission* (ou une fonction englobant celle-ci, telle que *i2c_lire_registre_nb*) doit être appelée jusqu'à ce qu'elle renvoie I2C_SUCCES. -Pour un exemple concret lisant une valeur dans une mémoire i2c, voir *test_i2c_lecture_pico_annex_nb2* de [Test.c](Test.c) \ No newline at end of file +Pour un exemple concret lisant une valeur dans une mémoire i2c, voir *test_i2c_lecture_pico_annex_nb2* de [Test.c](Test.c) + +Communication +===================== + +Carte Servomoteur +----------------- + +Discussion en I2C avec la carte servomoteur, celle-ci étant esclave. + +Registres: + +Adresse, RW, Fonction +* 0x00 (R) : Position du bras 1 +* 0x01 (R) : Position du bras 2 +* 0x02 (R) : Position du bras 3 +* 0x03 (R) : Position du bras 4 +* 0x04 (R) : Position du bras 5 +* 0x05 (R) : Position du bras 5 +* 0x06 (RW) : Action demandée / action en cours + + 'G' : Demande d'attraper plante et dépose à gauche + + 'g' : Action en cours: attraper plante et dépose à gauche + + 'D' : Demande d'attraper plante et dépose à droite + + 'd' : Action en cours: attraper plante et dépose à droite + + 't' : Action terminée + +Position du bras : 1 octet, découpé ainsi : + +- 4 premiers bits : position de l'aimant +- 4 derniers bits : hauteur du bras diff --git a/Test.c b/Test.c index 13c029d..35f5568 100644 --- a/Test.c +++ b/Test.c @@ -42,21 +42,14 @@ #define TEST_TIMEOUT_US 10000000 int test_APDS9960(void); -int test_localisation(void); int test_asser_position_avance(void); int test_asser_position_avance_et_tourne(int, int); int test_transition_gyro_pas_gyro(void); -int test_trajectoire(void); void affiche_localisation(void); -int test_aller_retour(); -int test_endurance_aller_retour(); -void test_trajectoire_teleplot(); int test_capteurs_balise(void); int test_geometrie(void); int test_angle_balise(void); -int continuous_printf = 1; - // Mode test : renvoie 0 pour quitter le mode test int mode_test(){ @@ -69,8 +62,6 @@ int mode_test(){ 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("N - Fonctions geometrique\n"); printf("O - Analyse obstacle\n"); printf("P - Asser Position - perturbation\n"); @@ -120,16 +111,7 @@ int mode_test(){ case 'j': while(test_asser_position_avance_et_tourne(0, 0)); break; - - case 'K': - case 'k': - while(test_aller_retour()); - break; - case 'L': - case 'l': - while(test_localisation()); - break; case 'N': case 'n': @@ -156,11 +138,6 @@ int mode_test(){ while(test_log()); break; - case 'T': - case 't': - while(test_trajectoire()); - break; - case 'U': case 'u': while(test_i2c()); @@ -170,11 +147,6 @@ int mode_test(){ case 'v': while(test_APDS9960()); break; - - case 'W': - case 'w': - while(test_endurance_aller_retour()); - break; case 'Z': case 'z': @@ -255,357 +227,6 @@ int test_APDS9960(){ return 1; } -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(); - } - -} - -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_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_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; - -} - - - -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 diff --git a/Test_i2c.c b/Test_i2c.c index b679965..8039ba5 100644 --- a/Test_i2c.c +++ b/Test_i2c.c @@ -290,7 +290,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){ enum i2c_resultat_t retour_i2c = I2C_EN_COURS; - + printf("A - Aimant tient pot\n"); + printf("C - Aimant lache pot\n"); printf("D - Deguisement On\n"); printf("E - Deguisement Off\n"); printf("F - Ferme porte\n"); @@ -321,6 +322,22 @@ int test_i2c_ecriture_pico_annex_nb_2(){ if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){ printf("lettre !\n"); switch(lettre){ + case 'a': + case 'A': + for(int i=1; i<7; i++){ + i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE); + } + printf("=> Lache pot\n"); + break; + + case 'c': + case 'C': + for(int i=1; i<7; i++){ + i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_TIENT); + } + printf("=> Tient pot\n"); + break; + case 'd': case 'D': i2c_annexe_active_deguisement(); diff --git a/Tests_deplacement.c b/Tests_deplacement.c index 0698188..61834a7 100644 --- a/Tests_deplacement.c +++ b/Tests_deplacement.c @@ -2,11 +2,18 @@ #include #include "pico/multicore.h" -#include "QEI.h" -#include "Moteurs.h" #include "Asser_Moteurs.h" -#include "Geometrie_robot.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); @@ -15,6 +22,12 @@ 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); + +bool continuous_printf=true; + #define TEST_TIMEOUT_US 10000000 // Mode test : renvoie 0 pour quitter le mode test @@ -23,6 +36,9 @@ int mode_test_deplacement(){ 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("D - Aller-retour - Endurance - avec gyroscope...\n"); stdio_flush(); @@ -39,6 +55,21 @@ int mode_test_deplacement(){ 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 PICO_ERROR_TIMEOUT: iteration--; if(iteration == 0){ @@ -204,4 +235,354 @@ int test_cde_vitesse_cercle(){ 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, 700, 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_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_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; + } \ No newline at end of file diff --git a/Trajet.c b/Trajet.c index bbaa24b..c2afe61 100644 --- a/Trajet.c +++ b/Trajet.c @@ -3,7 +3,9 @@ #include "Trajectoire.h" #include "Trajet.h" #include "Asser_Position.h" +#include "Asser_Moteurs.h" #include "Monitoring.h" +#include "Temps.h" float Trajet_calcul_vitesse(float temps_s); int Trajet_terminee(float abscisse); @@ -25,6 +27,8 @@ float vitesse_max_contrainte_obstacle; /// @brief Initialise le module Trajet. A appeler en phase d'initilisation void Trajet_init(){ + Temps_init(); + AsserMoteur_Init(); abscisse = 0; vitesse_mm_s = 0; position_mm = 0; diff --git a/i2c_annexe.c b/i2c_annexe.c index 598b3ec..709fa75 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -16,52 +16,73 @@ #define TAILLE_DONNEES_EMISSION 6 #define TAILLE_DONNEES_RECEPTION 15 - +#define ADRESSE_PIC18F4550 0x31 +#define ADRESSE_PIC18F4550_DEBUT_W 0x00 +#define TAILLE_DONNEES_PIC18F4550_EMISSION 7 uint8_t donnees_emission[TAILLE_DONNEES_EMISSION]; +uint8_t donnees_emission_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION]; uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION]; + uint donnees_a_envoyer=0; +uint donnees_a_envoyer_pic=0; void i2c_annexe_gestion(){ static enum { - EMISSION_DONNEES, - EMISSION_TEMPO, - RECEPTION_DONNEES - } etat_i2c_annexe=EMISSION_DONNEES; + EMISSION_DONNEES_RPi, + EMISSION_TEMPO_RPi, + RECEPTION_DONNEES_RPi, + EMISSION_PIC18F, + } etat_i2c_annexe=EMISSION_PIC18F; enum i2c_resultat_t retour_i2c; static uint32_t temps; const uint32_t tempo = 1000; switch(etat_i2c_annexe){ - case EMISSION_DONNEES: + case EMISSION_DONNEES_RPi: if(donnees_a_envoyer){ retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_W, donnees_emission, TAILLE_DONNEES_EMISSION); if(retour_i2c == I2C_SUCCES){ - etat_i2c_annexe = EMISSION_TEMPO; + etat_i2c_annexe = EMISSION_TEMPO_RPi; donnees_a_envoyer=0; temps = time_us_32(); } }else{ - etat_i2c_annexe = EMISSION_TEMPO; + etat_i2c_annexe = EMISSION_TEMPO_RPi; temps = time_us_32(); } break; - case EMISSION_TEMPO: + case EMISSION_TEMPO_RPi: if(temps + tempo < time_us_32() ){ - etat_i2c_annexe = RECEPTION_DONNEES; + etat_i2c_annexe = RECEPTION_DONNEES_RPi; } break; - case RECEPTION_DONNEES: + case RECEPTION_DONNEES_RPi: retour_i2c = i2c_lire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_R, donnees_reception, TAILLE_DONNEES_RECEPTION); if(retour_i2c == I2C_SUCCES){ - etat_i2c_annexe = EMISSION_DONNEES; + etat_i2c_annexe = EMISSION_PIC18F; temps = time_us_32(); } break; + + case EMISSION_PIC18F: + if(donnees_a_envoyer_pic){ + retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PIC18F4550, ADRESSE_PIC18F4550_DEBUT_W, + donnees_emission_pic18f4550, TAILLE_DONNEES_PIC18F4550_EMISSION); + if(retour_i2c == I2C_SUCCES){ + etat_i2c_annexe = EMISSION_DONNEES_RPi; + temps = time_us_32(); + donnees_a_envoyer_pic=0; + } + }else{ + etat_i2c_annexe = EMISSION_DONNEES_RPi; + } + break; + } } @@ -152,4 +173,17 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){ for (uint8_t capteur = 0; capteur < 12; capteur++){ distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R]; } -} \ No newline at end of file +} + +/// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs +/// @param actionneur de 1 à 6, pour le "bras" correspondant". +/// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h +/// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h +void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){ + if(actionneur < 1 || actionneur > 6){ + printf("Erreur: i2c_annexe_actionneur_pot\n" ); + return; + } + donnees_emission_pic18f4550[actionneur-1] = (pos_bras << 2) | pos_doigt; + donnees_a_envoyer_pic=1; +} diff --git a/i2c_annexe.h b/i2c_annexe.h index 70a4138..1a58ea8 100644 --- a/i2c_annexe.h +++ b/i2c_annexe.h @@ -3,6 +3,16 @@ #define CONTACTEUR_ACTIF 0 #define CONTACTEUR_INACTIF 1 +#define DOIGT_TIENT 1 +#define DOIGT_LACHE 0 + +#define BRAS_PLIE 0 +#define BRAS_POT_SOL 1 +#define BRAS_POT_POT 2 +#define BRAS_ECARTE 3 +#define BRAS_DEPOSE_JARDINIERE 4 +#define BRAS_HAUT 5 + void i2c_annexe_gestion(void); void i2c_annexe_active_turbine(void); void i2c_annexe_desactive_turbine(void); @@ -31,4 +41,6 @@ void i2c_annexe_desactive_deguisement(void); void i2c_annexe_plie_bras(void); void i2c_annexe_deplie_bras(void); -uint8_t i2c_annexe_get_tension_batterie(void); \ No newline at end of file +uint8_t i2c_annexe_get_tension_batterie(void); + +void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt);