From 2c4fe4a02e42ae757e1c72f5624c215eaff4d672 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 22 Dec 2022 22:35:49 +0100 Subject: [PATCH] =?UTF-8?q?D=C3=A9but=20des=20tests=20avec=20l'APDS=5F9960?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APDS_9960.c | 120 ++++++++- APDS_9960.h | 2 + CMakeLists.txt | 5 +- Holonome2023.c | 129 ++++++++++ Robot_config.c | 9 + Robot_config.h | 2 + Test.c | 652 +++++++++++++++++++++++++++++++++++++++++++++++++ Test.h | 1 + i2c_maitre.c | 61 +++++ i2c_maitre.h | 9 + 10 files changed, 987 insertions(+), 3 deletions(-) create mode 100644 Holonome2023.c create mode 100644 Robot_config.c create mode 100644 Robot_config.h create mode 100644 Test.c create mode 100644 Test.h create mode 100644 i2c_maitre.c create mode 100644 i2c_maitre.h diff --git a/APDS_9960.c b/APDS_9960.c index 7db55d2..710b1aa 100644 --- a/APDS_9960.c +++ b/APDS_9960.c @@ -1,10 +1,128 @@ -int APDS9960_Init(){ +#include "i2c_maitre.h" +#include "pico/stdlib.h" +#include + +#define APDS_ADRESSE 0x39 + + +inline void cc3_rgb2tls (uint8_t rouge, uint8_t vert, uint8_t bleu); + +void APDS9960_Init(){ + char registre; + char valeur; + + i2c_maitre_init(); + // Désactivation du module pour le configurer + registre = 0x80; + valeur = 0x0; + while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES); + + // Registres à configurer + // ATIME (0x81): temps d'acquisition + // | Valeur | Nb cycles | Temps | + // | 0 | 255 | 712 ms | + // | 182 | 72 | 200 ms | + // | 255 | 1 | 2,78 ms | + registre = 0x81; + valeur = 220; + while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES); + // CONTROL (0x8F<1:0>): Gain + // 0 : x1 + // 1 : x4 + // 2 : x16 + // 3 : x64 + registre = 0x8F; + valeur = 0x03; + while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES); + + // Temps d'attente : Wen, WTIME, WLONG // ENABLE (PON) Address : 0x80 - Après les autres registres !! // RESERVED | Gesture En | Prox. Interrupt En | ALS Interrupt En | Wait En | Proximity Detect En | ALS En | Power On | // Ob0000 0011 => 0x03 + registre = 0x80; + valeur = 0x3; + while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES); + +} + +int APDS9960_Lire(){ + unsigned char reception[8]; + uint16_t clear, rouge, vert, bleu; + i2c_lire_registre(APDS_ADRESSE, 0x94, reception, 8); + clear = (uint16_t) reception[0] + (uint16_t) reception[1] << 8; + rouge = (uint16_t) reception[2] + (uint16_t) reception[3] << 8; + vert = (uint16_t) reception[4] + (uint16_t) reception[5] << 8; + bleu = (uint16_t) reception[6] + (uint16_t) reception[7] << 8; + printf("clear:%u\n", clear/256); + printf("rouge:%u\n", rouge/256); + printf("vert:%u\n", vert/256); + printf("bleu:%u\n", bleu/256); + + + cc3_rgb2tls(rouge/256, vert/256, bleu/256); + + return 1; + +} + +inline void cc3_rgb2tls (uint8_t rouge, uint8_t vert, uint8_t bleu) +{ + uint8_t hue, sat, val; + uint8_t rgb_min, rgb_max; + rgb_max = rouge; + rgb_min = rouge; + if (rouge > rgb_max) + rgb_max = rouge; + if (vert > rgb_max) + rgb_max = vert; + if (bleu > rgb_max) + rgb_max = bleu; + if (rouge < rgb_min) + rgb_min = rouge; + if (vert < rgb_min) + rgb_min = vert; + if (bleu < rgb_min) + rgb_min = bleu; + +// compute V + val = rgb_max; + if (val == 0) { + printf("val:0"); + hue = sat = 0; + return; + } + +// compute S + sat = 255 * (rgb_max - rgb_min) / val; + if (sat == 0) { + printf("sat:0"); + return; + } + +// compute H + if (rgb_max == rouge) { + hue = + 0 + 43 * (int16_t)(vert - + bleu) / (rgb_max - rgb_min); + } + else if (rgb_max == vert) { + hue = + 85 + 43 * (int16_t)(bleu - + rouge) / (rgb_max - rgb_min); + } + else { /* rgb_max == blue */ + + hue = + 171 + 43 * (int16_t)(rouge - + vert) / (rgb_max - rgb_min); + } + + printf("val:%u\n", val); + printf("sat:%u\n", sat); + printf("teinte:%u\n", hue*360/250); } \ No newline at end of file diff --git a/APDS_9960.h b/APDS_9960.h index e69de29..b434c53 100644 --- a/APDS_9960.h +++ b/APDS_9960.h @@ -0,0 +1,2 @@ +void APDS9960_Init(void); +int APDS9960_Lire(void); \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c8e366..f76e32d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ QEI.c gyro.c gyro_L3GD20H.c gyro_ADXRS453.c +i2c_maitre.c Localisation.c Moteurs.c Robot_config.c @@ -36,9 +37,9 @@ add_definitions(-DGYRO_ADXRS453) pico_enable_stdio_usb(test 1) pico_enable_stdio_uart(test 1) pico_add_extra_outputs(test) -target_link_libraries(test pico_stdlib hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore) +target_link_libraries(test pico_stdlib hardware_i2c hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore) add_custom_target(Flash DEPENDS test - COMMAND sudo picotool load ${PROJECT_BINARY_DIR}/test.uf2 + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/test.uf2 ) \ No newline at end of file diff --git a/Holonome2023.c b/Holonome2023.c new file mode 100644 index 0000000..c789d82 --- /dev/null +++ b/Holonome2023.c @@ -0,0 +1,129 @@ +#include +#include "pico/multicore.h" +#include "pico/stdlib.h" +#include "hardware/gpio.h" +#include "hardware/timer.h" +#include "pico/binary_info.h" +#include "math.h" +#include "Test.h" + +#include "gyro.h" +#include "Asser_Moteurs.h" +#include "Asser_Position.h" +#include "Commande_vitesse.h" +#include "Localisation.h" +#include "Moteurs.h" +#include "QEI.h" +#include "Servomoteur.h" +#include "spi_nb.h" +#include "Temps.h" +#include "Trajectoire.h" +#include "Trajet.h" + +const uint LED_PIN = 25; +const uint LED_PIN_ROUGE = 28; +const uint LED_PIN_NE_PAS_UTILISER = 22; + +uint temps_cycle; + + +#define V_INIT -999.0 +#define TEST_TIMEOUT_US 10000000 + +int mode_test(); + +int main() { + bi_decl(bi_program_description("This is a test binary.")); + bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED")); + double vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT; + struct t_angle_gyro_double angle_gyro; + + uint32_t temps_ms = 0, temps_ms_old; + uint32_t temps_us_debut_cycle; + + stdio_init_all(); + + gpio_init(LED_PIN); + gpio_set_dir(LED_PIN, GPIO_OUT); + gpio_put(LED_PIN, 1); + + gpio_init(LED_PIN_ROUGE); + gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT); + gpio_put(LED_PIN_ROUGE, 1); + + // Il faut neutraliser cette broche qui pourrait interférer avec + // la lecture des codeurs. (problème sur la carte électrique)... + gpio_init(LED_PIN_NE_PAS_UTILISER); + gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN); + + sleep_ms(3000); + Servomoteur_Init(); + //puts("Debut"); + //spi_test(); + + //while(1); + Temps_init(); + Moteur_Init(); + QEI_init(); + AsserMoteur_Init(); + Localisation_init(); + + while(mode_test()); + + Gyro_Init(); + + + + temps_ms = Temps_get_temps_ms(); + temps_ms_old = temps_ms; + while (1) { + u_int16_t step_ms = 2; + float coef_filtre = 1-0.8; + + while(temps_ms == Temps_get_temps_ms()); + temps_ms = Temps_get_temps_ms(); + temps_ms_old = temps_ms; + + // Tous les pas de step_ms + if(!(temps_ms % step_ms)){ + temps_us_debut_cycle = time_us_32(); + Gyro_Read(step_ms); + + //gyro_affiche(gyro_get_vitesse(), "Angle :"); + // Filtre + angle_gyro = gyro_get_vitesse(); + if(vitesse_filtre_x == V_INIT){ + vitesse_filtre_x = angle_gyro.rot_x; + vitesse_filtre_y = angle_gyro.rot_y; + vitesse_filtre_z = angle_gyro.rot_z; + }else{ + vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre; + vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre; + vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre; + } + temps_cycle = time_us_32() - temps_us_debut_cycle; + + //printf("%#x, %#x\n", (double)temps_ms_old / 1000, vitesse_filtre_z); + + //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000)); + //gyro_affiche(angle_gyro, "Vitesse (°/s),"); + + } + + // Toutes les 50 ms + if((Temps_get_temps_ms() % 50) == 0){ + struct t_angle_gyro_double m_gyro; + m_gyro = gyro_get_angle_degres(); + printf("%f, %f, %d\n", (double)temps_ms / 1000, m_gyro.rot_z, temps_cycle); + } + + // Toutes les 500 ms + if((Temps_get_temps_ms() % 500) == 0){ + //gyro_affiche(gyro_get_angle(), "Angle :"); + } + // Toutes les secondes + if((Temps_get_temps_ms() % 500) == 0){ + //gyro_get_temp(); + } + } +} diff --git a/Robot_config.c b/Robot_config.c new file mode 100644 index 0000000..873d199 --- /dev/null +++ b/Robot_config.c @@ -0,0 +1,9 @@ +int position_avec_gyroscope = 0; + +int get_position_avec_gyroscope(void){ + return position_avec_gyroscope; +} + +void set_position_avec_gyroscope(int _use_gyro){ + position_avec_gyroscope = _use_gyro; +} \ No newline at end of file diff --git a/Robot_config.h b/Robot_config.h new file mode 100644 index 0000000..e1675fd --- /dev/null +++ b/Robot_config.h @@ -0,0 +1,2 @@ +int get_position_avec_gyroscope(void); +void set_position_avec_gyroscope(int _use_gyro); \ No newline at end of file diff --git a/Test.c b/Test.c new file mode 100644 index 0000000..0138d8e --- /dev/null +++ b/Test.c @@ -0,0 +1,652 @@ +#include +#include "pico/multicore.h" +#include "pico/stdlib.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 "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_rotation(void); +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_tca9535(void); +void affiche_localisation(void); + + +// 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 - rotation pure\n"); + printf("F - Commande en vitesse - carré\n"); + printf("G - Commande en vitesse - cercle\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("M - pour les moteurs\n"); + printf("L - pour la localisation\n"); + printf("T - Trajectoire\n"); + printf("U - Scan du bus i2c\n"); + printf("V - APDS_9960\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_rotation()); + break; + + case 'F': + case 'f': + while(test_cde_vitesse_rectangle()); + break; + + case 'G': + case 'g': + while(test_cde_vitesse_cercle()); + 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 'M': + case 'm': + while(test_moteurs()); + break; + case 'L': + case 'l': + while(test_localisation()); + break; + + case 'T': + case 't': + while(test_trajectoire()); + break; + + case 'U': + case 'u': + while(test_tca9535()); + break; + + case 'V': + case 'v': + while(test_APDS9960()); + 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; + +} + +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_tca9535(){ + // 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 = 0x39; + 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; + 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); + } + +} + +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); + break; + + case 'c': + case 'C': + Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250); + break; + + case 'd': + case 'D': + Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + break; + + default: return 0; + } + + Trajet_debut_trajectoire(trajectoire); + multicore_launch_core1(test_trajectoire_printf); + 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); + }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"); + 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_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); + + 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); + + 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); + + 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; + + printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n"); + do{ + QEI_update(); + Localisation_gestion(); + position = Localisation_get(); + printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654); + sleep_ms(100); + + lettre = getchar_timeout_us(0); + }while(lettre == PICO_ERROR_TIMEOUT); + + 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(TEST_TIMEOUT_US); + 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; +} diff --git a/Test.h b/Test.h new file mode 100644 index 0000000..019bb5e --- /dev/null +++ b/Test.h @@ -0,0 +1 @@ +int mode_test(); \ No newline at end of file diff --git a/i2c_maitre.c b/i2c_maitre.c new file mode 100644 index 0000000..185db44 --- /dev/null +++ b/i2c_maitre.c @@ -0,0 +1,61 @@ +#include "i2c_maitre.h" +#include "hardware/gpio.h" +#include "hardware/i2c.h" +#include "pico/stdlib.h" +#include + +#define I2C_SDA_PIN 16 +#define I2C_SCL_PIN 17 + +void i2c_maitre_init(void){ + stdio_init_all(); + i2c_init(i2c0, 100 * 1000); + + gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C); + + gpio_set_function(6, GPIO_FUNC_NULL); + gpio_set_function(7, GPIO_FUNC_NULL); + +} + +/// @brief Pour l'instant bloquant, mais devrait passer en non bloquant bientôt +/// @param adresse_7_bits +/// @param +/// @return 0: en cours, +int i2c_lire_registre(char adresse_7_bits, char registre, char * reception, char len){ + int statu; + char emission[1]; + + emission[0] = registre; + statu = i2c_write_blocking (i2c0, adresse_7_bits, emission, 1, 0); + if(statu == PICO_ERROR_GENERIC){ + printf("I2C - Envoi registre Echec\n"); + return I2C_ECHEC; + } + + statu = i2c_read_blocking (i2c0, adresse_7_bits, reception, len, 0); + if(statu == PICO_ERROR_GENERIC){ + printf("I2C - Lecture registre Echec\n"); + return I2C_ECHEC; + } + + return I2C_SUCCES; +} + +int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){ + int statu; + char emission[2]; + + emission[0] = registre; + emission[1] = valeur_registre; + statu = i2c_write_blocking (i2c0, adresse_7_bits, emission, 2, 0); + if(statu == PICO_ERROR_GENERIC){ + printf("Erreur ecrire registre\n"); + return I2C_ECHEC; + } + + printf("i2c Registre %x, valeur %x\n", registre, valeur_registre); + + return I2C_SUCCES; +} \ No newline at end of file diff --git a/i2c_maitre.h b/i2c_maitre.h new file mode 100644 index 0000000..a1ad172 --- /dev/null +++ b/i2c_maitre.h @@ -0,0 +1,9 @@ +#define I2C_EN_COURS 0 +#define I2C_SUCCES 1 +#define I2C_ECHEC -1 + + + +void i2c_maitre_init(void); +int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre); +int i2c_lire_registre(char adresse_7_bits, char registre, char * reception, char len); \ No newline at end of file