diff --git a/CMakeLists.txt b/CMakeLists.txt index 3356661..19693bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ gyro.c gyro_L3GD20H.c gyro_ADXRS453.c i2c_maitre.c +i2c_annexe.c Localisation.c Moteurs.c Robot_config.c diff --git a/Holonome2023.c b/Holonome2023.c index c789d82..c770d92 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -49,7 +49,7 @@ int main() { gpio_init(LED_PIN_ROUGE); gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT); - gpio_put(LED_PIN_ROUGE, 1); + gpio_put(LED_PIN_ROUGE, 0); // Il faut neutraliser cette broche qui pourrait interférer avec // la lecture des codeurs. (problème sur la carte électrique)... diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 166eca3..7f419e4 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -7,6 +7,7 @@ // Rotation en rad/s pour accoster les cerises #define ROTATION_CERISE 0.5f + double vitesse_accostage_mm_s=100; enum etat_action_t cerise_accostage(void){ @@ -21,14 +22,6 @@ enum etat_action_t cerise_accostage(void){ CERISE_ACCOSTE } etat_accostage=CERISE_AVANCE_DROIT; - - // Position des contacteurs - contacteur_gauche.x_mm = RAYON_ROBOT; - contacteur_gauche.y_mm = 0; - - contacteur_droit.x_mm = RAYON_ROBOT * cos(-M_PI/6); - contacteur_droit.y_mm = RAYON_ROBOT * sin(-M_PI/6); - switch (etat_accostage) { case CERISE_AVANCE_DROIT: @@ -52,3 +45,10 @@ enum etat_action_t cerise_accostage(void){ return etat_action; } +void commande_rotation_contacteur_longer_A(){ + commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0); +} + +void commande_rotation_contacteur_longer_C(){ + commande_rotation(-ROTATION_CERISE, RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2); +} \ No newline at end of file diff --git a/Test.c b/Test.c index 2a82916..2d2789c 100644 --- a/Test.c +++ b/Test.c @@ -14,6 +14,7 @@ #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" @@ -50,6 +51,7 @@ 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(); @@ -75,6 +77,7 @@ int mode_test(){ 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(); @@ -164,6 +167,11 @@ int mode_test(){ 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){ @@ -221,7 +229,7 @@ int test_APDS9960(){ int test_i2c_lecture_pico_annex(){ i2c_maitre_init(); uint8_t tampon[10]; - uint8_t registre=0; + uint8_t registre=2; uint8_t adresse = 0x17; int ret; @@ -251,7 +259,7 @@ int test_i2c_lecture_pico_annex(){ int test_i2c_lecture_pico_annex_nb(){ i2c_maitre_init(); uint8_t tampon[10]; - uint8_t registre=0; + uint8_t registre=2; uint8_t adresse = 0x17; uint32_t time_i2c[5]; const uint8_t T_MAX_I2C = 10; @@ -334,7 +342,7 @@ int test_i2c_lecture_pico_annex_nb2(){ i2c_maitre_init(); uint8_t tampon[10]; - uint8_t registre=0; + uint8_t registre=8; uint8_t adresse = 0x17; uint32_t time_i2c[5]; const uint8_t T_MAX_I2C = 10; @@ -385,6 +393,9 @@ int test_i2c_ecriture_pico_annex_nb(){ 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"); + printf("Q pour quitter\n"); int lettre; do{ @@ -418,6 +429,23 @@ int test_i2c_ecriture_pico_annex_nb(){ 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; } @@ -438,6 +466,103 @@ int test_i2c_ecriture_pico_annex_nb(){ 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(); } diff --git a/i2c_annexe.c b/i2c_annexe.c new file mode 100644 index 0000000..df7ff94 --- /dev/null +++ b/i2c_annexe.c @@ -0,0 +1,90 @@ +#include "i2c_annexe.h" +#include "i2c_maitre.h" +#include "stdio.h" + +#define ADRESSE_PICO_ANNEXE 0x17 +#define ADRESSE_DEBUT_W 0x08 +#define ADRESSE_DEBUT_R 0x0A +#define ADRESSE_TURBINE_PORTE 0x0A +#define ADRESSE_CONTACTEURS 0x0B +#define TAILLE_DONNEES_EMISSION 3 +#define TAILLE_DONNEES_RECEPTION 13 + +uint donnees_a_envoyer=0; + +uint8_t donnees_emission[TAILLE_DONNEES_EMISSION]; +uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION]; + +void i2c_annexe_gestion(){ + static enum { + EMISSION_DONNEES, + RECEPTION_DONNEES + + } etat_i2c_annexe=EMISSION_DONNEES; + enum i2c_resultat_t retour_i2c; + + switch(etat_i2c_annexe){ + case EMISSION_DONNEES: + 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 = RECEPTION_DONNEES; + donnees_a_envoyer=0; + } + }else{ + etat_i2c_annexe = RECEPTION_DONNEES; + } + break; + + case RECEPTION_DONNEES: + 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; + + } + break; + } + +} + +void i2c_annexe_active_turbine(void){ + donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01; + donnees_a_envoyer=1; +} +void i2c_annexe_desactive_turbine(void){ + donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE; + donnees_a_envoyer=1; +} + +void i2c_annexe_ouvre_porte(void){ + donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFD; + donnees_a_envoyer=1; +} +void i2c_annexe_ferme_porte(void){ + donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x02; + donnees_a_envoyer=1; +} + +void i2c_annexe_active_propulseur(void){ + donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x04; + donnees_a_envoyer=1; +} +void i2c_annexe_desactive_propulseur(void){ + donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFB; + donnees_a_envoyer=1; +} + +uint8_t i2c_annexe_get_contacteur_butee_A(void){ + return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 3) & 0x01; +} + +uint8_t i2c_annexe_get_contacteur_butee_C(void){ + return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] & 0x01); +} + +uint8_t i2c_annexe_get_contacteur_longer_A(void){ + return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 2) & 0x01; +} +uint8_t i2c_annexe_get_contacteur_longer_C(void){ + return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 1) & 0x01; +} \ No newline at end of file diff --git a/i2c_annexe.h b/i2c_annexe.h new file mode 100644 index 0000000..d7d8e77 --- /dev/null +++ b/i2c_annexe.h @@ -0,0 +1,17 @@ +#include "pico/stdlib.h" + +void i2c_annexe_gestion(void); +void i2c_annexe_active_turbine(void); +void i2c_annexe_desactive_turbine(void); + +void i2c_annexe_ouvre_porte(void); +void i2c_annexe_ferme_porte(void); + +void i2c_annexe_active_propulseur(void); +void i2c_annexe_desactive_propulseur(void); + +uint8_t i2c_annexe_get_contacteur_butee_A(void); +uint8_t i2c_annexe_get_contacteur_butee_C(void); + +uint8_t i2c_annexe_get_contacteur_longer_A(void); +uint8_t i2c_annexe_get_contacteur_longer_C(void);