From 8dfe505a4485e6d516ce330c08fc77dea62e9ac7 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 9 Mar 2025 21:38:20 +0100 Subject: [PATCH] Passage sur Xiao RP2040 --- .vscode/c_cpp_properties.json | 1 + Asser_Moteurs.c | 90 ------- Asser_Moteurs.h | 8 - Asser_Position.c | 78 ------- Asser_Position.h | 6 - CMakeLists.txt | 40 ++-- Commande_vitesse.c | 27 --- Commande_vitesse.h | 2 - Localisation.c | 54 ----- Localisation.h | 10 - Moteurs.c | 98 -------- Moteurs.h | 14 -- Platform/platform.c | 197 +++++++++------- QEI.c | 101 -------- QEI.h | 16 -- Trajectoire.c | 167 ------------- Trajectoire.h | 40 ---- Trajectoire_bezier.c | 35 --- Trajectoire_bezier.h | 5 - Trajectoire_circulaire.c | 26 --- Trajectoire_circulaire.h | 4 - Trajectoire_droite.c | 17 -- Trajectoire_droite.h | 4 - Trajet.c | 220 ----------------- Trajet.h | 37 --- VL53L8CX_ULD_API/src/vl53l8cx_api.c | 10 + VL53L8_2024.c | 72 +++++- VL53L8_2024.h | 1 + main.c | 351 +++++++++++----------------- quadrature_encoder.pio | 165 ------------- 30 files changed, 332 insertions(+), 1564 deletions(-) delete mode 100644 Asser_Moteurs.c delete mode 100644 Asser_Moteurs.h delete mode 100644 Asser_Position.c delete mode 100644 Asser_Position.h delete mode 100644 Commande_vitesse.c delete mode 100644 Commande_vitesse.h delete mode 100644 Localisation.c delete mode 100644 Localisation.h delete mode 100644 Moteurs.c delete mode 100644 Moteurs.h delete mode 100644 QEI.c delete mode 100644 QEI.h delete mode 100644 Trajectoire.c delete mode 100644 Trajectoire.h delete mode 100644 Trajectoire_bezier.c delete mode 100644 Trajectoire_bezier.h delete mode 100644 Trajectoire_circulaire.c delete mode 100644 Trajectoire_circulaire.h delete mode 100644 Trajectoire_droite.c delete mode 100644 Trajectoire_droite.h delete mode 100644 Trajet.c delete mode 100644 Trajet.h delete mode 100644 quadrature_encoder.pio diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 05ca5f8..7560cfd 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -3,6 +3,7 @@ "myDefaultIncludePath": [ "${workspaceFolder}", "${workspaceFolder}/build", + "${myDefaultIncludePath}/Platform/VL53L8CX_ULD_API/inc", "${env:PICO_SDK_PATH}/src/**/include", "${env:PICO_SDK_PATH}/src/common/pico_base/include", "${env:PICO_SDK_PATH}/build/generated/pico_base", diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c deleted file mode 100644 index 0098144..0000000 --- a/Asser_Moteurs.c +++ /dev/null @@ -1,90 +0,0 @@ -#include "QEI.h" -#include "Moteurs.h" -#include "Asser_Moteurs.h" - -#define ASSERMOTEUR_GAIN_P 300000.f -#define ASSERMOTEUR_GAIN_I 30000.f - -float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) -float commande_I[3]; // Terme integral - -void AsserMoteur_Init(int id){ - QEI_init(id); - Moteur_Init(); - for(unsigned int i =0; i< 2; i ++){ - commande_I[i]=0; - consigne_mm_s[i]=0; - } -} - -/// @brief Défini une consigne de vitesse pour le moteur indiqué. -/// @param moteur : Moteur à asservir -/// @param _consigne_mm_s : consigne de vitesse en mm/s -void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){ - consigne_mm_s[moteur] = _consigne_mm_s; - -} - -/// @brief Envoie la consigne du moteur -/// @param moteur : Moteur à asservir -float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){ - return consigne_mm_s[moteur]; -} - - - -float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ - enum QEI_name_t qei; - float distance, temps; - switch (moteur) - { - case MOTEUR_A: qei = QEI_A_NAME; break; - case MOTEUR_B: qei = QEI_B_NAME; break; - default: break; - } - distance = QEI_get_mm(qei); - temps = step_ms / 1000.0f; - - return distance / temps; -} - -/// @brief Indique si le robot est à l'arrêt -/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses) -/// @return 1 si le robot est immobile, 0 s'il est en mouvement. -uint32_t AsserMoteur_RobotImmobile(int step_ms){ - const float seuil_vitesse_immobile_mm_s = 0.1; - if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s && - AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s ){ - return 1; - } - return 0; -} - -/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement -/// @param step_ms -void AsserMoteur_Gestion(int step_ms){ - // Pour chaque moteur - for(uint moteur=MOTEUR_A; moteur 32760) {commande = 32760;} - if(commande < -32760) {commande = -32760;} - - Moteur_SetVitesse(moteur, commande); - - } -} \ No newline at end of file diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h deleted file mode 100644 index 42adbda..0000000 --- a/Asser_Moteurs.h +++ /dev/null @@ -1,8 +0,0 @@ -#include "Moteurs.h" - -uint32_t AsserMoteur_RobotImmobile(int step_ms); -void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s); -float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur); -float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms); -void AsserMoteur_Gestion(int step_ms); -void AsserMoteur_Init(int); \ No newline at end of file diff --git a/Asser_Position.c b/Asser_Position.c deleted file mode 100644 index 74870ff..0000000 --- a/Asser_Position.c +++ /dev/null @@ -1,78 +0,0 @@ -#include "Localisation.h" -#include "Commande_vitesse.h" -#include "math.h" - -#define GAIN_P_POSITION 15 -#define GAIN_P_ORIENTATION 10 - -#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN) - -struct position_t position_maintien; - -/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot -/// C'est à la consigne d'être défini avant pour être atteignable. -/// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms); -/// @param position_consigne : position à atteindre dans le référentiel de la table. -float delta_x_mm, delta_y_mm, delta_orientation_radian; -float delta_orientation_radian_tmp; -void Asser_Position(struct position_t position_consigne){ - float delta_avance_mm; - float avance_mm_s, rotation_radian_s; - //float delta_x_mm, delta_y_mm, delta_orientation_radian; - struct position_t position_actuelle; - float delta_orientation_radian_tmp; - - position_actuelle = Localisation_get(); - - // Calcul de l'erreur - delta_x_mm = position_consigne.x_mm - position_actuelle.x_mm; - delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm; - delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm); - delta_orientation_radian_tmp = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian; - delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp); - - // On asservi sur +PI/2 / -PI/2 - if(delta_orientation_radian > (M_PI/2)){ - delta_orientation_radian -= M_PI; - delta_avance_mm = -delta_avance_mm; - } - if(delta_orientation_radian < -(M_PI/2)){ - delta_orientation_radian += M_PI; - delta_avance_mm = -delta_avance_mm; - } - - - - // Asservissement - avance_mm_s = delta_avance_mm * GAIN_P_POSITION; - rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION; - - if(delta_avance_mm < 10){ - rotation_radian_s=0; - } - - // Commande en vitesse - commande_vitesse(avance_mm_s, rotation_radian_s); - -} - -void Asser_Position_set_Pos_Maintien(struct position_t position){ - position_maintien=position; -} - -void Asser_Position_maintien(){ - Asser_Position(position_maintien); -} - -float Asser_Position_get_erreur_angle(){ - return delta_orientation_radian; -} - -/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil -/// @return 1 si panic, 0 si nominal -int Asser_Position_panic_angle(){ - if(delta_orientation_radian > MAX_ERREUR_ANGLE){ - return 1; - } - return 0; -} diff --git a/Asser_Position.h b/Asser_Position.h deleted file mode 100644 index aa2e802..0000000 --- a/Asser_Position.h +++ /dev/null @@ -1,6 +0,0 @@ -#include "Geometrie.h" -void Asser_Position(struct position_t position_consigne); -void Asser_Position_set_Pos_Maintien(struct position_t position); -void Asser_Position_maintien(); - -int Asser_Position_panic_angle(); diff --git a/CMakeLists.txt b/CMakeLists.txt index f9f23d3..cb6362c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,30 +2,23 @@ cmake_minimum_required(VERSION 3.13) include(pico_sdk_import.cmake) -project(Mon_Projet C CXX ASM) +project(VL53L8X_Gradin C CXX ASM) set(CMAKE_C_STNDARD 11) set(CMAKE_CXX_STANDARD 17) set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) +set(PICO_BOARD seeed_xiao_rp2040) + + pico_sdk_init() -add_executable(Mon_Projet - Asser_Position.c - Asser_Moteurs.c - Commande_vitesse.c + +add_executable(VL53L8X_Gradin Geometrie.c i2c_maitre.c - Moteurs.c - Localisation.c main.c - QEI.c Temps.c - Trajectoire_bezier.c - Trajectoire_circulaire.c - Trajectoire_droite.c - Trajectoire.c - Trajet.c VL53L8CX_ULD_API/src/vl53l8cx_api.c VL53L8CX_ULD_API/src/vl53l8cx_plugin_detection_thresholds.c VL53L8CX_ULD_API/src/vl53l8cx_plugin_motion_indicator.c @@ -34,25 +27,22 @@ add_executable(Mon_Projet Platform/platform.c ) -pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) +target_include_directories(VL53L8X_Gradin PRIVATE VL53L8CX_ULD_API/inc/) -target_include_directories(Mon_Projet PRIVATE VL53L8CX_ULD_API/inc/) - -target_link_libraries(Mon_Projet +target_link_libraries(VL53L8X_Gradin hardware_adc - hardware_i2c - hardware_pwm - hardware_pio + hardware_spi + hardware_i2c pico_stdlib pico_multicore ) -pico_enable_stdio_usb(Mon_Projet 1) -pico_enable_stdio_uart(Mon_Projet 1) +pico_enable_stdio_usb(VL53L8X_Gradin 1) +pico_enable_stdio_uart(VL53L8X_Gradin 1) -pico_add_extra_outputs(Mon_Projet) +pico_add_extra_outputs(VL53L8X_Gradin) add_custom_target(Flash - DEPENDS Mon_Projet - COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2 + DEPENDS VL53L8X_Gradin + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/VL53L8X_Gradin.uf2 ) diff --git a/Commande_vitesse.c b/Commande_vitesse.c deleted file mode 100644 index c831fee..0000000 --- a/Commande_vitesse.c +++ /dev/null @@ -1,27 +0,0 @@ -#include "Asser_Moteurs.h" -#include "Geometrie_robot.h" -#include "Commande_vitesse.h" - - - - - -/// @brief Commande de la vitesse dans le référentiel du robot -/// @param avance_mm_s : Vitesse d'avance -/// @param orientation_radian_s : Rotation en radian/s -void commande_vitesse(float avance_mm_s, float orientation_radian_s){ - float vitesse_roue_gauche, vitesse_roue_droite; - - vitesse_roue_gauche = avance_mm_s - (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM); - vitesse_roue_droite = avance_mm_s + (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM); - - AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_roue_droite); - AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_gauche); - -} - -/// @brief Arrête le robot. -void commande_vitesse_stop(){ - AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); - AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0); -} \ No newline at end of file diff --git a/Commande_vitesse.h b/Commande_vitesse.h deleted file mode 100644 index 609078f..0000000 --- a/Commande_vitesse.h +++ /dev/null @@ -1,2 +0,0 @@ -void commande_vitesse(float vitesse_avance_mm_s, float orientation_radian_s); -void commande_vitesse_stop(void); \ No newline at end of file diff --git a/Localisation.c b/Localisation.c deleted file mode 100644 index e2e735e..0000000 --- a/Localisation.c +++ /dev/null @@ -1,54 +0,0 @@ -#include "Localisation.h" -#include "Temps.h" -#include "QEI.h" -#include "math.h" -#include "Geometrie_robot.h" - -struct position_t position; - -void Localisation_init(int id){ - Temps_init(); - QEI_init(id); - position.x_mm = 0; - position.y_mm = 0; - position.angle_radian = 0; -} - -void Localisation_set(float x_mm, float y_mm, float angle_radian){ - position.x_mm = x_mm; - position.y_mm = y_mm; - position.angle_radian = angle_radian; -} - -void Localisation_set_x(float x_mm){ - position.x_mm = x_mm; -} - -void Localisation_set_y(float y_mm){ - position.y_mm = y_mm; -} - -void Localisation_set_angle(float angle_radian){ - position.angle_radian = angle_radian; -} - -void Localisation_gestion(){ - float distance_roue_gauche_mm = QEI_get_mm(QEI_B_NAME); - float distance_roue_droite_mm = QEI_get_mm(QEI_A_NAME); - - float delta_avance_mm, delta_orientation_rad; - - delta_avance_mm = (distance_roue_droite_mm + distance_roue_gauche_mm)/2; - delta_orientation_rad = (distance_roue_droite_mm - distance_roue_gauche_mm) / (DISTANCE_ROUES_CENTRE_MM*2); - - position.angle_radian += delta_orientation_rad; - - // Projection dans le référentiel de la table - position.x_mm += delta_avance_mm * cosf(position.angle_radian); - position.y_mm += delta_avance_mm * sinf(position.angle_radian); - -} - -struct position_t Localisation_get(void){ - return position; -} diff --git a/Localisation.h b/Localisation.h deleted file mode 100644 index de101f6..0000000 --- a/Localisation.h +++ /dev/null @@ -1,10 +0,0 @@ -#include "Geometrie.h" - -struct position_t Localisation_get(void); -void Localisation_gestion(); -void Localisation_init(int); - -void Localisation_set(float x_mm, float y_mm, float angle_radian); -void Localisation_set_x(float x_mm); -void Localisation_set_y(float y_mm); -void Localisation_set_angle(float angle_radian); \ No newline at end of file diff --git a/Moteurs.c b/Moteurs.c deleted file mode 100644 index a3dc9a8..0000000 --- a/Moteurs.c +++ /dev/null @@ -1,98 +0,0 @@ -#include "hardware/pwm.h" -#include "Moteurs.h" - -#define MOTEUR_A 0 -#define MOTEUR_B 1 -#define MOTEUR_C 2 - -#define MOTEUR_A_VITESSE 6 -#define MOTEUR_B_VITESSE 8 -#define MOTEUR_C_VITESSE 10 - -#define MOTEUR_A_SENS 5 -#define MOTEUR_B_SENS 7 -#define MOTEUR_C_SENS 9 - -#define M1_SENS1 7 -#define M1_SENS2 13 -#define M1_VITESSE 27 //5B -#define M2_SENS1 10 -#define M2_SENS2 5 -#define M2_VITESSE 9 //4B - -uint slice_moteur_A, slice_moteur_B, slice_moteur_C; - -/// @brief Initialisation les entrées / sorties requises pour les moteurs -void Moteur_Init(){ - gpio_init(M1_SENS1); - gpio_init(M1_SENS2); - gpio_init(M2_SENS1); - gpio_init(M2_SENS2); - gpio_set_dir(M1_SENS1, GPIO_OUT); - gpio_set_dir(M1_SENS2, GPIO_OUT); - gpio_set_dir(M2_SENS1, GPIO_OUT); - gpio_set_dir(M2_SENS2, GPIO_OUT); - gpio_put(M1_SENS1, 0); - gpio_put(M1_SENS2, 0); - gpio_put(M2_SENS1, 0); - gpio_put(M2_SENS2, 0); - - gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM); - gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM); - pwm_set_wrap(5, (uint16_t)65535); - pwm_set_wrap(4, (uint16_t)65535); - pwm_set_chan_level(5, PWM_CHAN_B, 0); - pwm_set_chan_level(4, PWM_CHAN_B, 0); - pwm_set_enabled(5, true); - pwm_set_enabled(4, true); - - Moteur_SetVitesse(MOTEUR_A, 0); - Moteur_SetVitesse(MOTEUR_B, 0); -} - - -/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur -/// @param moteur : Moteur (voir enum t_moteur) -/// @param vitesse : Vitesse entre -32767 et 32767 -void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){ - uint16_t u_vitesse; - - // Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 15 + signe) - if (vitesse < 0){ - u_vitesse = -vitesse; - }else{ - u_vitesse = vitesse; - } - u_vitesse = u_vitesse * 2; - - switch(moteur){ - case MOTEUR_A: - pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse); - if(vitesse > 0){ - gpio_put(M1_SENS1, 0); - gpio_put(M1_SENS2, 1); - }else{ - gpio_put(M1_SENS1, 1); - gpio_put(M1_SENS2, 0); - } - break; - - case MOTEUR_B: - pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse); - if(vitesse < 0){ - gpio_put(M2_SENS1, 0); - gpio_put(M2_SENS2, 1); - }else{ - gpio_put(M2_SENS1, 1); - gpio_put(M2_SENS2, 0); - } - break; - - } - -} - -void Moteur_Stop(void){ - Moteur_SetVitesse(MOTEUR_A, 0); - Moteur_SetVitesse(MOTEUR_B, 0); -} \ No newline at end of file diff --git a/Moteurs.h b/Moteurs.h deleted file mode 100644 index 1eed7da..0000000 --- a/Moteurs.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "pico/stdlib.h" - -#ifndef MOTEURS_H -#define MOTEURS_H -enum t_moteur { - MOTEUR_A=0, - MOTEUR_B=1, - MOTEUR_C=2 -}; -#endif - -void Moteur_Init(void); -void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ); -void Moteur_Stop(void); diff --git a/Platform/platform.c b/Platform/platform.c index 209d1dc..ec356af 100644 --- a/Platform/platform.c +++ b/Platform/platform.c @@ -13,101 +13,56 @@ #include "platform.h" #include "pico/stdlib.h" -#include "hardware/i2c.h" +#include "hardware/spi.h" #include "hardware/gpio.h" #include +#include "vl53l8cx_api.h" -#define I2C_SUCCESS 0 -#define I2C_FAILED 1 -#define I2C_BUFFER_EXCEEDED 2 +#define BUFFER_SIZE 0x1000 +#define ERR_OK 0 -#define I2C_DEVICE i2c1 - -#define MAX_I2C_BUFFER 0x8100 - - -/// @brief Blocking function allowing to write a register on an I2C device -/// @param address_7_bits -/// @param index : register to write -/// @param values : values to write -/// @param count : number of byte to send -/// @return 0: Success, -1 or -2: Failed -int8_t i2c_write_register(char adresse_7_bits, uint16_t index, uint8_t * values, uint32_t count){ - int statu; - uint8_t buffer[MAX_I2C_BUFFER]; - uint8_t index_to_unint8[2]; - absolute_time_t timeout_time; - - if(count > MAX_I2C_BUFFER - 2){ - return I2C_BUFFER_EXCEEDED; - } - - index_to_unint8[0] = (index >> 8) & 0xFF; - index_to_unint8[1] = index & 0xFF; - - buffer[0] = index_to_unint8[0]; - buffer[1] = index_to_unint8[1]; - - for(uint32_t i=0; i> 8) & 0xFF; - index_to_unint8[1] = index & 0xFF; - - statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, index_to_unint8, 2, 0); - if(statu == PICO_ERROR_GENERIC){ - printf("I2C - Write - Envoi registre Echec %x\n", adresse_7_bits); - return I2C_FAILED; - } - - statu = i2c_read_blocking (I2C_DEVICE, adresse_7_bits, pdata, count, 0); - if(statu == PICO_ERROR_GENERIC){ - printf("I2C - Lecture registre Echec\n"); - return I2C_FAILED; - } - - return I2C_SUCCESS; +void SPI_endTransaction(char my_ss_pin){ + //SPI.endTransaction(); + gpio_put(my_ss_pin, 1); + //SPI.end(); } - uint8_t RdByte( VL53L8CX_Platform *p_platform, uint16_t RegisterAdress, uint8_t *p_value) { uint8_t status = 255; + uint8_t my_ss_pin; + uint8_t tampon[2]; + my_ss_pin = p_platform->address; /* Need to be implemented by customer. This function returns 0 if OK */ - return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_value, 1); + //Serial.printf("RdByte adresse :%x\n",p_platform->address); + SPI_beginTransaction(my_ss_pin); + + tampon[0] = RegisterAdress >> 8 & 0x7F; + tampon[1] = RegisterAdress & 0xFF; + + spi_write_blocking(spi0, tampon, 2); + spi_read_blocking(spi0, 0x00, p_value, 1 ); + + //printf("RdByte 0x%02X%02X 0x%02X\n", tampon[0], tampon[1], *p_value); + + SPI_endTransaction(my_ss_pin); + + return ERR_OK; + } uint8_t WrByte( @@ -116,9 +71,26 @@ uint8_t WrByte( uint8_t value) { uint8_t status = 255; + uint8_t my_ss_pin; + uint8_t tampon[3]; + + my_ss_pin = p_platform->address; /* Need to be implemented by customer. This function returns 0 if OK */ - return i2c_write_register(p_platform->address >> 1, RegisterAdress, &value, 1); + SPI_beginTransaction(my_ss_pin); + + tampon[0] = RegisterAdress >> 8 | 0x80; + tampon[1] = RegisterAdress & 0xFF; + tampon[2] = value; + + //printf("WrByte 0x%02X%02X 0x%02X\n", tampon[0], tampon[1], tampon[2]); + + spi_write_blocking(spi0, tampon, 3); + + SPI_endTransaction(my_ss_pin); + + + return ERR_OK; } @@ -128,9 +100,32 @@ uint8_t WrMulti( uint8_t *p_values, uint32_t size) { - uint8_t status = 255; - - return i2c_write_register(p_platform->address >> 1, RegisterAdress, p_values, size); + uint32_t index =0; + uint8_t my_buffer[BUFFER_SIZE]; + uint8_t tampon[2]; + uint8_t my_ss_pin; + my_ss_pin = p_platform->address; + + SPI_beginTransaction(my_ss_pin); + + tampon[0] = RegisterAdress >> 8 | 0x80; + tampon[1] = RegisterAdress & 0xFF; + spi_write_blocking(spi0, tampon, 2); + + while(size > BUFFER_SIZE){ + memcpy(my_buffer, (uint8_t*)&p_values[index], BUFFER_SIZE); + spi_write_blocking(spi0, my_buffer, BUFFER_SIZE); + size = size - BUFFER_SIZE; + index = index + BUFFER_SIZE; + } + if(size > 0){ + memcpy(my_buffer, (uint8_t*)&p_values[index], size); + spi_write_blocking(spi0, my_buffer, size); + } + + SPI_endTransaction(my_ss_pin); + + return ERR_OK; } @@ -141,8 +136,36 @@ uint8_t RdMulti( uint32_t size) { uint8_t status = 255; - - return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_values, size); + uint32_t index =0; + uint32_t buffer_size = 32; + uint8_t my_ss_pin; + uint8_t tampon[2]; + my_ss_pin = p_platform->address; + + SPI_beginTransaction(my_ss_pin); + + tampon[0] = RegisterAdress >> 8 & 0x7F; + tampon[1] = RegisterAdress & 0xFF; + + spi_write_blocking(spi0, tampon, 2); + spi_read_blocking(spi0, 0, p_values, size); + + //printf("RdMulti 0x%02X%02X size : 0x%02X\n", tampon[0], tampon[1], size); + +/* + while(size > buffer_size){ + spi_read_blocking(spi0, 0, &(p_values[index]), buffer_size); + index += buffer_size; + size -= buffer_size; + } + if(size > 0){ + spi_read_blocking(spi0, 0, &(p_values[index]), size); + } + */ + + SPI_endTransaction(my_ss_pin); + + return ERR_OK; } @@ -193,6 +216,6 @@ uint8_t WaitMs( { /* Need to be implemented by customer. This function returns 0 if OK */ sleep_ms(TimeMs); - + //delay(TimeMs); return 0; -} +} \ No newline at end of file diff --git a/QEI.c b/QEI.c deleted file mode 100644 index c95dbe3..0000000 --- a/QEI.c +++ /dev/null @@ -1,101 +0,0 @@ -#include -#include "pico/stdlib.h" -#include "hardware/pio.h" -#include "hardware/timer.h" -#include "QEI.h" -#include "quadrature_encoder.pio.h" - - -/*** C'est ici que se fait la conversion en mm - * ***/ - -// Roues 60 mm de diamètre, 188,5 mm de circonférence -// Réduction Moteur 30:1 -// Réduction poulie 16:12 -// Nombre d'impulsions par tour moteur : 200 -// Nombre d'impulsions par tour réducteur : 6000 -// Nombre d'impulsions par tour de roue : 8000 -// Impulsion / mm : 42,44 - -#define IMPULSION_PAR_MM_50_1 (12.45f) -#define IMPULSION_PAR_MM_30_1 (7.47f) - -float impulsion_par_mm; - - -struct QEI_t QEI_A, QEI_B; - -bool QEI_est_init = false; - -PIO pio_QEI = pio0; - - -void QEI_init(int identifiant){ - // Initialisation des 3 modules QEI - // Chaque module QEI sera dans une machine à état du PIO 0 - if(!QEI_est_init){ - // Offset le début du programme - // Si ce n'est pas 0, le programme ne marchera pas - uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program); - if(offset != 0){ - printf("PIO init error: offset != 0"); - } - // Initialisation des "machines à états" : - // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer - quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 0); - // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer - quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0); - - QEI_A.value=0; - QEI_B.value=0; - QEI_est_init=true; - } - impulsion_par_mm = IMPULSION_PAR_MM_50_1; - if(identifiant == 0 || identifiant >= 4){ - impulsion_par_mm = IMPULSION_PAR_MM_30_1; - } - -} - -/// @brief Lit les modules QEI et stock l'écart en cette lecture et la lecture précédente. -void QEI_update(void){ - - int old_value; - - old_value = QEI_A.value; - QEI_A.value = quadrature_encoder_get_count(pio_QEI, 0); - QEI_A.delta = QEI_A.value - old_value; - - old_value = QEI_B.value; - QEI_B.value = quadrature_encoder_get_count(pio_QEI, 1); - QEI_B.delta = QEI_B.value - old_value; - -} - -/// @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 -/// @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){ - switch (qei) - { - case QEI_A_NAME: - return QEI_A.delta; - break; - - case QEI_B_NAME: - return -QEI_B.delta; - break; - - default: - break; - } -} - -/// @brief Renvoi la distance parcourue en mm depuis la lecture précédente -/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) -/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update() -float QEI_get_mm(enum QEI_name_t qei){ - return ((float) QEI_get(qei)) / impulsion_par_mm; -} \ No newline at end of file diff --git a/QEI.h b/QEI.h deleted file mode 100644 index e031360..0000000 --- a/QEI.h +++ /dev/null @@ -1,16 +0,0 @@ -struct QEI_t{ - int value; - int delta; -}; - -enum QEI_name_t{ - QEI_A_NAME=0, - QEI_B_NAME=1, -}; - -extern struct QEI_t QEI_A, QEI_B, QEI_C; - -void QEI_update(void); -void QEI_init(int); -int QEI_get(enum QEI_name_t qei); -float QEI_get_mm(enum QEI_name_t qei); \ No newline at end of file diff --git a/Trajectoire.c b/Trajectoire.c deleted file mode 100644 index c099490..0000000 --- a/Trajectoire.c +++ /dev/null @@ -1,167 +0,0 @@ -#include "Trajectoire.h" -#include "Trajectoire_bezier.h" -#include "Trajectoire_circulaire.h" -#include "Trajectoire_droite.h" - -#include "math.h" - -#define NB_MAX_TRAJECTOIRES 5 -#define PRECISION_ABSCISSE 0.001f - - -void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon, - float orientation_debut_rad, float orientation_fin_rad){ - trajectoire->type = TRAJECTOIRE_CIRCULAIRE; - trajectoire->p1.x = centre_x; - trajectoire->p1.y = centre_y; - trajectoire->angle_debut_degre = angle_debut_degre; - trajectoire->angle_fin_degre = angle_fin_degre; - trajectoire->rayon = rayon; - trajectoire->longueur = -1; - trajectoire->orientation_debut_rad = orientation_debut_rad; - trajectoire->orientation_fin_rad = orientation_fin_rad; -} - -void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, - float orientation_debut_rad, float orientation_fin_rad){ - trajectoire->type = TRAJECTOIRE_DROITE; - trajectoire->p1.x = p1_x; - trajectoire->p1.y = p1_y; - trajectoire->p2.x = p2_x; - trajectoire->p2.y = p2_y; - trajectoire->longueur = -1; - trajectoire->orientation_debut_rad = orientation_debut_rad; - trajectoire->orientation_fin_rad = orientation_fin_rad; -} - -void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, - float orientation_debut_rad, float orientation_fin_rad){ - trajectoire->type = TRAJECTOIRE_BEZIER; - trajectoire->p1.x = p1_x; - trajectoire->p1.y = p1_y; - trajectoire->p2.x = p2_x; - trajectoire->p2.y = p2_y; - trajectoire->p3.x = p3_x; - trajectoire->p3.y = p3_y; - trajectoire->p4.x = p4_x; - trajectoire->p4.y = p4_y; - trajectoire->longueur = -1; - trajectoire->orientation_debut_rad = orientation_debut_rad; - trajectoire->orientation_fin_rad = orientation_fin_rad; -} - -void Trajectoire_inverse(struct trajectoire_t * trajectoire){ - struct trajectoire_t old_trajectoire; - old_trajectoire = *trajectoire; - - trajectoire->orientation_debut_rad = old_trajectoire.orientation_fin_rad; - trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad; - - if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){ - trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre; - trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre; - return; - } - if(trajectoire->type == TRAJECTOIRE_DROITE){ - trajectoire->p1 = old_trajectoire.p2; - trajectoire->p2 = old_trajectoire.p1; - return; - } - if(trajectoire->type == TRAJECTOIRE_BEZIER){ - trajectoire->p1 = old_trajectoire.p4; - trajectoire->p2 = old_trajectoire.p3; - trajectoire->p3 = old_trajectoire.p2; - trajectoire->p4 = old_trajectoire.p1; - } - - - -} - -/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin -/// @param trajectoire -/// @return Longueur de la trajectoire -float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ - if(trajectoire->longueur > 0){ - // La longueur est déjà calculée - }else{ - // Calculons la longueur de la trajectoire - switch(trajectoire->type){ - case TRAJECTOIRE_DROITE: - Trajectoire_droite_get_longueur(trajectoire); - break; - case TRAJECTOIRE_CIRCULAIRE: - Trajectoire_circulaire_get_longueur(trajectoire); - break; - - case TRAJECTOIRE_BEZIER: - Trajectoire_bezier_get_longueur(trajectoire); - break; - } - } - return trajectoire->longueur; -} - -/// @brief Renvoie le point d'une trajectoire à partir de son abscisse -/// @param abscisse : abscisse sur la trajectoire -/// @return point en coordonnées X/Y -struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){ - struct point_xyo_t point_xyo; - switch(trajectoire->type){ - case TRAJECTOIRE_DROITE: - point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse); - point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse); - break; - - case TRAJECTOIRE_CIRCULAIRE: - point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse); - point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse); - break; - - case TRAJECTOIRE_BEZIER: - point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse); - point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse); - break; - - } - return point_xyo; -} - -float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){ - return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) trajectoire->orientation_fin_rad * abscisse; -} - -/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée -/// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire -/// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire -/// @return nouvelle abscisse -float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){ - double delta_abscisse, delta_mm, erreur_relative; - - if(distance_mm == 0){ - return abscisse; - } - // Ceci permet d'avoir une abscisse exact sur les trajectoires droites, les trajectoires circulaires et les rotations - delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire); - if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE || trajectoire->type == TRAJECTOIRE_DROITE){ - return abscisse + delta_abscisse; - } - - delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy ); - - // Sur les trajectoires de bézier, il peut être nécessaire d'affiner - // Les cas où l'algorythme diverge ne devraient pas se produire car distance_cm << longeur_trajectoire. - erreur_relative = 1 - delta_mm / distance_mm; - while(fabs(erreur_relative) > PRECISION_ABSCISSE){ - delta_abscisse = delta_abscisse * distance_mm / delta_mm; - delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy ); - erreur_relative = 1 - delta_mm / distance_mm; - } - - return abscisse + delta_abscisse; -} - -double distance_points(struct point_xy_t point, struct point_xy_t point_old){ - return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2)); - -} diff --git a/Trajectoire.h b/Trajectoire.h deleted file mode 100644 index aecda2b..0000000 --- a/Trajectoire.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef TRAJECTOIRE_H -#define TRAJECTOIRE_H - -enum trajectoire_type_t{ - TRAJECTOIRE_DROITE, - TRAJECTOIRE_CIRCULAIRE, - TRAJECTOIRE_BEZIER, -}; - -struct point_xy_t{ - double x, y; -}; - -struct point_xyo_t{ - struct point_xy_t point_xy; - float orientation; -}; - -struct trajectoire_t { - enum trajectoire_type_t type; - struct point_xy_t p1, p2, p3, p4; - float orientation_debut_rad, orientation_fin_rad; - float rayon, angle_debut_degre, angle_fin_degre; - float longueur; -}; - -float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); -struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); -float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse); -float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm); -double distance_points(struct point_xy_t point, struct point_xy_t point_old); -void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, - float rayon, float orientation_debut_rad, float orientation_fin_rad); -void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad); -void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, - float orientation_debut_rad, float orientation_fin_rad); -void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad); -void Trajectoire_inverse(struct trajectoire_t * trajectoire); - -#endif diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c deleted file mode 100644 index 5c4aeab..0000000 --- a/Trajectoire_bezier.c +++ /dev/null @@ -1,35 +0,0 @@ -#include "Trajectoire.h" -#include "Trajectoire_bezier.h" - - -void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){ - struct point_xy_t point, point_old; - float nb_pas=500; - - trajectoire->longueur=0; - point_old = trajectoire->p1; - - for(float abscisse=0; abscisse<=1; abscisse += 1./nb_pas){ - point = Trajectoire_bezier_get_point(trajectoire, abscisse); - trajectoire->longueur += distance_points(point, point_old); - point_old = point; - } -} - - -/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse -/// @param abscisse : compris entre 0 et 1 -struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){ - struct point_xy_t point; - point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + - 3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) + - 3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) + - (double) trajectoire->p4.x * abscisse * abscisse * abscisse; - - point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + - 3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) + - 3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) + - (double) trajectoire->p4.y * abscisse * abscisse * abscisse; - - return point; -} \ No newline at end of file diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h deleted file mode 100644 index 5930684..0000000 --- a/Trajectoire_bezier.h +++ /dev/null @@ -1,5 +0,0 @@ -#include "Trajectoire.h" - - -void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse); \ No newline at end of file diff --git a/Trajectoire_circulaire.c b/Trajectoire_circulaire.c deleted file mode 100644 index 2ec8726..0000000 --- a/Trajectoire_circulaire.c +++ /dev/null @@ -1,26 +0,0 @@ -#include "math.h" -#include "Trajectoire.h" - - -void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){ - float distance_angulaire; - if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){ - distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre; - }else{ - distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre; - } - trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.; -} - -/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse -/// @param abscisse : compris entre 0 et 1 -struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){ - struct point_xy_t point; - float angle_degre; - - angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse; - point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon; - point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon; - - return point; -} diff --git a/Trajectoire_circulaire.h b/Trajectoire_circulaire.h deleted file mode 100644 index 6229d88..0000000 --- a/Trajectoire_circulaire.h +++ /dev/null @@ -1,4 +0,0 @@ -#include "math.h" - -void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float avancement); diff --git a/Trajectoire_droite.c b/Trajectoire_droite.c deleted file mode 100644 index a2fa2a8..0000000 --- a/Trajectoire_droite.c +++ /dev/null @@ -1,17 +0,0 @@ -#include "Trajectoire.h" - - -void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire){ - trajectoire->longueur = distance_points(trajectoire->p1, trajectoire->p2); -} - -/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse -/// @param abscisse : compris entre 0 et 1 -struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse){ - struct point_xy_t point; - - point.x = (float) trajectoire->p1.x * (1. - abscisse) + (float) trajectoire->p2.x * abscisse; - point.y = (float) trajectoire->p1.y * (1. - abscisse) + (float) trajectoire->p2.y * abscisse; - - return point; -} \ No newline at end of file diff --git a/Trajectoire_droite.h b/Trajectoire_droite.h deleted file mode 100644 index e4ed24c..0000000 --- a/Trajectoire_droite.h +++ /dev/null @@ -1,4 +0,0 @@ -#include "Trajectoire.h" - -void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire); -struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c deleted file mode 100644 index 7808b21..0000000 --- a/Trajet.c +++ /dev/null @@ -1,220 +0,0 @@ -#include -#include "Geometrie.h" -#include "Trajectoire.h" -#include "Trajet.h" -#include "Asser_Position.h" -#include "Asser_Moteurs.h" -#include "Temps.h" - -float Trajet_calcul_vitesse(float temps_s); -int Trajet_terminee(float abscisse); - -float abscisse; // Position entre 0 et 1 sur la trajectoire -float position_mm; // Position en mm sur la trajectoire -float vitesse_mm_s; -float vitesse_max_trajet_mm_s=500; -float acceleration_mm_ss; -const float acceleration_mm_ss_obstacle = 500; -struct trajectoire_t trajet_trajectoire; -struct position_t position_consigne; - -float distance_obstacle_mm; -float distance_fin_trajectoire_mm; -const float distance_pas_obstacle = 2000; - -float vitesse_max_contrainte_obstacle; - -/// @brief Initialise le module Trajet. A appeler en phase d'initilisation -void Trajet_init(int id){ - Temps_init(); - AsserMoteur_Init(id); - abscisse = 0; - vitesse_mm_s = 0; - position_mm = 0; - Trajet_config(TRAJECT_CONFIG_STD); -} - -/// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets -/// @param _vitesse_max_trajet_mm_s -/// @param _acceleration_mm_ss -void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss){ - vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s; - acceleration_mm_ss = _acceleration_mm_ss; -} - -void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){ - abscisse = 0; - vitesse_mm_s = 0; - position_mm = 0; - trajet_trajectoire = trajectoire; - Trajet_set_obstacle_mm(DISTANCE_INVALIDE); -} - -/// @brief Avance la consigne de position sur la trajectoire -/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde -/// @return TRAJET_EN_COURS ou TRAJET_TERMINE -struct point_xyo_t point; -enum etat_trajet_t Trajet_avance(float pas_de_temps_s){ - float distance_mm; - enum etat_trajet_t trajet_etat = TRAJET_EN_COURS; - - struct position_t position; - - // Calcul de la vitesse - vitesse_mm_s = Trajet_calcul_vitesse(pas_de_temps_s); - - // Calcul de l'avancement en mm - distance_mm = vitesse_mm_s * pas_de_temps_s; - position_mm += distance_mm; - - // Calcul de l'abscisse sur la trajectoire - abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm); - //set_debug_varf(abscisse); - - // Obtention du point consigne - point = Trajectoire_get_point(&trajet_trajectoire, abscisse); - - position.x_mm = point.point_xy.x; - position.y_mm = point.point_xy.y; - position.angle_radian = point.orientation; - - position_consigne=position; - Asser_Position(position); - - if(Trajet_terminee(abscisse)){ - Asser_Position_set_Pos_Maintien(position); - trajet_etat = TRAJET_TERMINE; - } - return trajet_etat; - -} - -void Trajet_stop(float pas_de_temps_s){ - vitesse_mm_s = 0; - Trajet_avance(0); -} - -/// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier -/// où les approximations font que l'abscisse peut ne pas atteindre 1. -/// @param abscisse : abscisse sur la trajectoire -/// @return 1 si le trajet est terminé, 0 sinon -int Trajet_terminee(float abscisse){ - /*if(abscisse >= 0.99 ){ - return 1; - }*/ - - if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){ - if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){ - return 1; - } - }else{ - if(abscisse >= 0.99 ){ - return 1; - } - } - return 0; -} - -/// @brief Envoie la consigne de position calculée par le module trajet. Principalement pour le débug/réglage asservissement. -struct position_t Trajet_get_consigne(){ - return position_consigne; -} - -/// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire -/// @param pas_de_temps_s : temps écoulé en ms -/// @return vitesse déterminée en m/s -float Trajet_calcul_vitesse(float pas_de_temps_s){ - float vitesse_max_contrainte; - float distance_contrainte,distance_contrainte_obstacle; - float vitesse; - // Calcul de la vitesse avec acceleration - vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s; - - // Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s) - // https://poivron-robotique.fr/Consigne-de-vitesse.html - distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm; - distance_fin_trajectoire_mm=distance_contrainte; - // En cas de dépassement, on veut garder la contrainte, pour l'instant - if(distance_contrainte > 0){ - vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte); - }else{ - vitesse_max_contrainte = 0; - } - - distance_contrainte_obstacle = Trajet_get_obstacle_mm(); - if(distance_contrainte_obstacle != DISTANCE_INVALIDE){ - vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); - if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){ - vitesse_max_contrainte = vitesse_max_contrainte_obstacle; - } - }/* - if((Trajet_get_obstacle_mm() != DISTANCE_INVALIDE) && (Trajet_get_obstacle_mm() < 50)){ - vitesse = 0; - }*/ - - - // Selection de la vitesse la plus faible - if(vitesse > vitesse_max_contrainte){ - vitesse = vitesse_max_contrainte; - } - if(vitesse > vitesse_max_trajet_mm_s){ - vitesse = vitesse_max_trajet_mm_s; - } - return vitesse; -} - - -float Trajet_get_obstacle_mm(void){ - return distance_obstacle_mm; -} - -void Trajet_set_obstacle_mm(float distance_mm){ - distance_obstacle_mm = distance_mm; -} - - -/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain -/// @return angle en radian. -float Trajet_get_orientation_avance(){ - struct point_xyo_t point, point_suivant; - float avance_abscisse = 0.01; - float angle; - - if(abscisse >= 1){ - return 0; - } - if(abscisse + avance_abscisse >= 1){ - avance_abscisse = 1 - abscisse; - } - - point = Trajectoire_get_point(&trajet_trajectoire, abscisse); - point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse); - - angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x); - return angle; -} - -void Trajet_inverse(){ - float old_abscisse = abscisse; - float old_position_mm = position_mm; - Trajectoire_inverse(&trajet_trajectoire); - Trajet_debut_trajectoire(trajet_trajectoire); - abscisse = 1 - old_abscisse; - position_mm = Trajectoire_get_longueur_mm(&trajet_trajectoire) - old_position_mm; -} - -float Trajet_get_abscisse(){ - return abscisse; -} - -/// @brief Indique si le robot est bloqué sur le trajet -/// @return 0 si le robot n'est pas bloqué, 1 s'il est bloqué -uint32_t Trajet_get_bloque(){ - if(Trajet_get_obstacle_mm() == DISTANCE_INVALIDE){ - return 0; - } - if (vitesse_max_contrainte_obstacle == 0){ - return 1; - } - return 0; -} \ No newline at end of file diff --git a/Trajet.h b/Trajet.h deleted file mode 100644 index 84438ac..0000000 --- a/Trajet.h +++ /dev/null @@ -1,37 +0,0 @@ -#include "pico/stdlib.h" -#include "Trajectoire.h" - -#ifndef TRAJET_H -#define TRAJET_H - -enum etat_trajet_t{ - TRAJET_EN_COURS, - TRAJET_TERMINE -}; - -// Vitesse et acceleration pour translation pure (en mm/s et mm/s²) -#define TRAJECT_CONFIG_RAPIDE 300, 1200 -#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200 -// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²) -#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 -// Vitesse et acceleration - standard (en mm et mm/s²) -#define TRAJECT_CONFIG_STD 300, 300 -// Vitesse et acceleration pour une rotation (rad/s et rad/s²) -#define TRAJECT_CONFIG_ROTATION_PURE 2, 2 - -extern const float distance_pas_obstacle; - -void Trajet_init(int); -void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss); -void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); -enum etat_trajet_t Trajet_avance(float temps_s); -struct position_t Trajet_get_consigne(void); -float Trajet_get_obstacle_mm(void); -void Trajet_set_obstacle_mm(float distance_mm); -void Trajet_stop(float); -float Trajet_get_orientation_avance(void); -float Trajet_get_abscisse(); -uint32_t Trajet_get_bloque(); -void Trajet_inverse(); - -#endif diff --git a/VL53L8CX_ULD_API/src/vl53l8cx_api.c b/VL53L8CX_ULD_API/src/vl53l8cx_api.c index 88347b6..eda29fb 100644 --- a/VL53L8CX_ULD_API/src/vl53l8cx_api.c +++ b/VL53L8CX_ULD_API/src/vl53l8cx_api.c @@ -11,6 +11,7 @@ */ #include +#include #include #include "vl53l8cx_api.h" #include "vl53l8cx_buffers.h" @@ -50,6 +51,7 @@ static uint8_t _vl53l8cx_poll_for_answer( { timeout++; } + printf("expected %d / %d\n", expected_value, (p_dev->temp_buffer[pos] & mask )); }while ((p_dev->temp_buffer[pos] & mask) != expected_value); return status; @@ -236,6 +238,7 @@ uint8_t vl53l8cx_is_alive( if((device_id == (uint8_t)0xF0) && (revision_id == (uint8_t)0x0C)) { *p_is_alive = 1; + printf("device_id:%d,revision_id:%d\n",device_id, revision_id); } else { @@ -259,6 +262,7 @@ uint8_t vl53l8cx_init( p_dev->is_auto_stop_enabled = (uint8_t)0x0; /* SW reboot sequence */ + printf("Rebbot sequence\n"); status |= WrByte(&(p_dev->platform), 0x7fff, 0x00); status |= WrByte(&(p_dev->platform), 0x0009, 0x04); status |= WrByte(&(p_dev->platform), 0x000F, 0x40); @@ -292,6 +296,7 @@ uint8_t vl53l8cx_init( status |= WrByte(&(p_dev->platform), 0x7fff, 0x02); /* Enable FW access */ + printf("Enable FW access\n"); status |= WrByte(&(p_dev->platform), 0x7fff, 0x01); status |= WrByte(&(p_dev->platform), 0x06, 0x01); status |= _vl53l8cx_poll_for_answer(p_dev, 1, 0, 0x21, 0xFF, 0x4); @@ -327,6 +332,7 @@ uint8_t vl53l8cx_init( status |= WrByte(&(p_dev->platform), 0x20, 0x06); /* Download FW into VL53L8CX */ + printf("Download FW\n"); status |= WrByte(&(p_dev->platform), 0x7fff, 0x09); status |= WrMulti(&(p_dev->platform),0, (uint8_t*)&VL53L8CX_FIRMWARE[0],0x8000); @@ -339,6 +345,7 @@ uint8_t vl53l8cx_init( status |= WrByte(&(p_dev->platform), 0x7fff, 0x01); /* Check if FW correctly downloaded */ + printf("Check FW\n"); status |= WrByte(&(p_dev->platform), 0x7fff, 0x01); status |= WrByte(&(p_dev->platform), 0x06, 0x03); @@ -366,6 +373,7 @@ uint8_t vl53l8cx_init( status |= WrByte(&(p_dev->platform), 0x7fff, 0x02); /* Firmware checksum */ + printf("Check checksum\n"); status |= RdMulti(&(p_dev->platform), (uint16_t)(0x812FFC & 0xFFFF), p_dev->temp_buffer, 4); SwapBuffer(p_dev->temp_buffer, 4); @@ -376,6 +384,8 @@ uint8_t vl53l8cx_init( } //crc_checksum = *((uint32_t *)&p_dev->temp_buffer[0]); + printf("More stuff\n"); + /* Get offset NVM data and store them into the offset buffer */ status |= WrMulti(&(p_dev->platform), 0x2fd8, (uint8_t*)VL53L8CX_GET_NVM_CMD, sizeof(VL53L8CX_GET_NVM_CMD)); diff --git a/VL53L8_2024.c b/VL53L8_2024.c index cdde1f5..fb5c593 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -1,6 +1,9 @@ #include "vl53l8cx_api.h" -#include "Trajet.h" #include +#include + +#define DEGREE_EN_RADIAN (M_PI / 180.) +#define PLAGE_ANGLE_DEGREE (40*DEGREE_EN_RADIAN) int masque[64]={ @@ -9,11 +12,23 @@ int masque[64]={ 3000,3000,3000,3000,3000,3000,3000,3000, 3000,3000,3000,3000,3000,3000,3000,3000, 3000,3000,3000,3000,3000,3000,3000,3000, - 350,350,350,350,350,350,350,350, - 281,286,299,306,310,314,302,286, - 195,200,202,206,213,200,200,202 + 3000,3000,3000,3000,3000,3000,3000,3000, + 3000,3000,3000,3000,3000,3000,3000,3000, + 3000,3000,3000,3000,3000,3000,3000,3000, }; + +float angles_VL53L8[]={ + 7./16. * PLAGE_ANGLE_DEGREE, + 5./16. * PLAGE_ANGLE_DEGREE, + 3./16. * PLAGE_ANGLE_DEGREE, + 1./16. * PLAGE_ANGLE_DEGREE, + -1./16. * PLAGE_ANGLE_DEGREE, + -3./16. * PLAGE_ANGLE_DEGREE, + -5./16. * PLAGE_ANGLE_DEGREE, + -7./16. * PLAGE_ANGLE_DEGREE + }; + float old_min_distance; void VL53L8_init(VL53L8CX_Configuration * Dev){ @@ -145,8 +160,8 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results printf("%d,", Results->distance_mm[col+ 8*row]); } } - printf("\n"); - */ + printf("\n");*/ + } int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ @@ -165,8 +180,51 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ *distance = min_distance-50; } old_min_distance = *distance; +} + +/// @brief Renvoie la position de la planche +/// @param Results Valeurs brutes du capteurs +/// @param pos_x position de la planche +/// @return 0 sucess, 1 failed +int VL53L8_pos_planche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y){ + uint16_t mesure1, mesure2; + int min_distance = 2000; + int col, row; + int distance_ref_mm = 200; + int last_col_avec_planche=0; + // Controle si la planche est à gauche + for(col=0; col<8; col++){ + printf("%d,", Results.distance_mm[col + 8*3]); + for(row=3; row<=3; row++){ + // SI c'est la première mesure, on la prend comme référence + if(distance_ref_mm >= 500){ + distance_ref_mm = Results.distance_mm[col + 8*row]; + }else{ + // Si on est plus proche que la distance de référence, on la prend comme référence. + if(distance_ref_mm > Results.distance_mm[col + 8*row]){ + distance_ref_mm = Results.distance_mm[col + 8*row]; + } + // Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche + if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){ + last_col_avec_planche = col; + // Double break; + col = 9; + break; + } + } + } + } + printf("\n"); - + if(last_col_avec_planche == 0){ + // Echec + *pos_x = 0; + return 1; + } + *pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]); + *pos_y = (float)distance_ref_mm; + + return 0; } float VL53L8_get_old_min_distance(){ diff --git a/VL53L8_2024.h b/VL53L8_2024.h index 6bcaf0c..4f6a7df 100644 --- a/VL53L8_2024.h +++ b/VL53L8_2024.h @@ -3,5 +3,6 @@ void VL53L8_init(VL53L8CX_Configuration * Dev); void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results); int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance); +int VL53L8_pos_planche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y); float VL53L8_get_old_min_distance(void); \ No newline at end of file diff --git a/main.c b/main.c index cedc1c1..a07a851 100644 --- a/main.c +++ b/main.c @@ -6,21 +6,13 @@ #include "pico/stdlib.h" #include "pico/multicore.h" #include "hardware/adc.h" -#include "hardware/pwm.h" -#include "Asser_Position.h" -#include "Asser_Moteurs.h" -#include "Commande_vitesse.h" +#include "hardware/spi.h" #include "i2c_maitre.h" -#include "Localisation.h" -#include "Moteurs.h" #include "Temps.h" -#include "Trajectoire.h" -#include "Trajet.h" #include "VL53L8_2024.h" #include "vl53l8cx_api.h" #include #include -#include "QEI.h" #define LED1PIN 20 #define TIRETTE_PIN 6 @@ -29,13 +21,21 @@ #define COULEUR_BLEU 1 #define COULEUR_JAUNE 0 +// XIAO RP2040 +#define SCK 2 +#define MISO 4 +#define MOSI 3 +#define D0 26 +#define D1 27 +#define D2 28 +#define D3 29 + + void affichage(void); void gestion_affichage(void); void tension_batterie_init(void); uint16_t tension_batterie_lire(void); -void identifiant_init(void); -uint identifiant_lire(void); int get_tirette(int); int get_couleur(void); @@ -52,27 +52,28 @@ extern struct point_xyo_t point; float vitesse; VL53L8CX_Configuration Dev; +uint8_t capteur_init(VL53L8CX_Configuration * capteur); + void main(void) { int ledpower = 500; + uint8_t tampon[10]; + uint8_t temp, status; VL53L8CX_ResultsData Results; bool fin_match = false; stdio_init_all(); + stdio_set_translate_crlf(&stdio_usb, false); Temps_init(); //tension_batterie_init(); - identifiant_init(); - Localisation_init(identifiant_lire()); - Trajet_init(identifiant_lire()); - i2c_maitre_init(); + spi_init(spi0, 2000000); uint32_t temps_ms = Temps_get_temps_ms(); uint32_t temps_depart_ms; - struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0}; float vitesse_mm_s=100; @@ -80,79 +81,141 @@ void main(void) gpio_set_dir(LED1PIN, GPIO_OUT ); gpio_put(LED1PIN, 1); - - //multicore_launch_core1(gestion_affichage); - multicore_launch_core1(gestion_VL53L8CX); + // Initialisation de la liaison SPI + gpio_set_function(2, GPIO_FUNC_SPI); + gpio_set_function(3, GPIO_FUNC_SPI); + gpio_set_function(4, GPIO_FUNC_SPI); + + gpio_set_function(16, GPIO_FUNC_NULL); + gpio_set_function(17, GPIO_FUNC_NULL); + gpio_set_function(18, GPIO_FUNC_NULL); + gpio_set_function(19, GPIO_FUNC_NULL); + + gpio_init(1); // CS + gpio_set_dir(1, GPIO_OUT ); + gpio_put(1, 1); + + Dev.platform.address = 1; + + spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST); + spi_init(spi0, 2000000); + + tampon[0] = 0x55; + tampon[1] = 0x55; + sleep_ms(5000); printf("Demarrage...\n"); + //multicore_launch_core1(gestion_affichage); + + + gestion_VL53L8CX(); + - configure_trajet(identifiant_lire(), get_couleur()); +} - - float vitesse_init =300; - vitesse = vitesse_init; - enum etat_trajet_t etat_trajet=TRAJET_EN_COURS; +uint8_t capteur_init(VL53L8CX_Configuration * capteur){ + uint8_t status=0; + printf("debut init\n"); + //pinMode(capteur->platform.address, OUTPUT); - while(get_tirette(identifiant_lire())); - gpio_put(LED1PIN, 0); + status = vl53l8cx_init(capteur); + if(status != 0){ + while(1){ + printf("Status init = %d\n", status); + WaitMs(&(capteur->platform), 1000); + break; + } + } + sleep_ms(100); - // Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part - if(identifiant_lire() == 3){ - sleep_ms(90000); + status = vl53l8cx_set_resolution(capteur, VL53L8CX_RESOLUTION_8X8); + if(status !=0){ + while(1){ + printf("vl53l8cx_set_resolution failed :%d\n", status); + WaitMs(&(capteur->platform), 1000); + break; + } } - temps_depart_ms = Temps_get_temps_ms(); - - while(1){ - - // Fin du match - if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){ - Moteur_Stop(); - while(1); + status = vl53l8cx_set_ranging_frequency_hz(capteur, 15); + if(status !=0){ + while(1){ + printf("vl53l8cx_set_ranging_frequency_hz (15hz) failed :%d\n", status); + WaitMs(&(capteur->platform), 1000); + break; } - if(temps_ms != Temps_get_temps_ms()){ - temps_ms = Temps_get_temps_ms(); - if(temps_ms % step_ms == 0){ - QEI_update(); - Localisation_gestion(); - - if(etat_trajet != TRAJET_TERMINE){ - etat_trajet = Trajet_avance((float)step_ms/1000.); - }else{ - Asser_Position_maintien(); - if(Asser_Position_panic_angle()){ - fin_match=1; - } - } - AsserMoteur_Gestion(step_ms); - } - } - - - } + + //vl53l8cx_set_target_order(&Dev, VL53L8CX_TARGET_ORDER_CLOSEST); + vl53l8cx_set_target_order(capteur, VL53L8CX_TARGET_ORDER_STRONGEST); + + status = vl53l8cx_start_ranging(capteur); + + printf("Capteur : %d, fin init: %d\n", capteur->platform.address, status); + return status; + +} + +uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){ + uint8_t isReady, status; + + status = vl53l8cx_check_data_ready(capteur, &isReady); + if(status){ + printf("erreur:%d\n", status); + } + if(isReady) + { + printf(">r%d:1\n",capteur->platform.address); + status = vl53l8cx_get_ranging_data(capteur, results); + if(status){ + printf("erreur:%d\n", status); + } + }else{ + //Serial.printf(">r:0\n"); + } + return isReady; +} + +void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_ResultsData * results_droit){ + uint8_t row, col; + for(col=0; col<8; col++){ + for(row=0; row<8; row++){ + printf("%4d ", results_gauche->distance_mm[col*8 + row]); + } + printf(" - "); + for(row=0; row<8; row++){ + printf("%4d ", results_droit->distance_mm[col*8 + row]); + } + + printf("\n"); + } + printf("\n"); } /// @brief Obtient la distance de l'obstacle le plus proche. /// @param + +float planche_pos_x, planche_pos_y; void gestion_VL53L8CX(void){ VL53L8CX_ResultsData Results; float distance_obstacle; - VL53L8_init(&Dev); + capteur_init(&Dev); sleep_ms(100); - VL53L8_lecture( &Dev, &Results); // une première lecture + capteur_actualise( &Dev, &Results); // une première lecture uint8_t status, isReady; while(1){ - status = vl53l8cx_check_data_ready(&Dev, &isReady); + isReady = capteur_actualise( &Dev, &Results); if(isReady){ - VL53L8_lecture( &Dev, &Results); + capteurs_affiche_donnees(&Results, &Results); + //VL53L8_lecture( &Dev, &Results); VL53L8_min_distance(Results, &distance_obstacle); - Trajet_set_obstacle_mm(distance_obstacle); + VL53L8_pos_planche(Results, &planche_pos_x, &planche_pos_y); } - affichage(); + //affichage(); + sleep_ms(50); } } @@ -166,9 +229,11 @@ void gestion_affichage(void){ } void affichage(void){ + printf(">planche_pox_x:%f\n",planche_pos_x); + printf(">planche_pox_y:%f\n",planche_pos_y); /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ - printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); + /*printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm()); printf(">abscisse:%f\n",abscisse); @@ -179,161 +244,5 @@ void affichage(void){ printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm); printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y); - printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire())); -} - -void tension_batterie_init(void){ - adc_init(); - adc_gpio_init(28); // Analog_2 - adc_select_input(2); - adc_run(1); // Free running mode -} - -uint16_t tension_batterie_lire(){ - uint16_t result = (uint16_t) adc_hw->result; - const float conversion_factor = 3.3f / (1 << 12); - float v_bat = result * conversion_factor * 11.; - - return result; -} - -void identifiant_init(){ - gpio_init(21); - gpio_init(22); - gpio_init(26); - gpio_pull_up(21); - gpio_pull_up(22); - gpio_pull_up(26); - gpio_set_dir(21, GPIO_IN); - gpio_set_dir(22, GPIO_IN); - gpio_set_dir(26, GPIO_IN); - - // Tirette - gpio_init(TIRETTE_PIN); - gpio_pull_up(TIRETTE_PIN); - gpio_set_dir(TIRETTE_PIN, GPIO_IN); - - // Couleur - gpio_init(COULEUR_PIN); - gpio_pull_up(COULEUR_PIN); - gpio_set_dir(COULEUR_PIN, GPIO_IN); -} - -int get_tirette(int id){ - if(id == 3){ - return !gpio_get(TIRETTE_PIN); - } - return (VL53L8_get_old_min_distance() <50); - -} - -int get_couleur(void){ - return gpio_get(COULEUR_PIN); -} - -/// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse -/// @return identifiant du robot (PDI switch) -uint identifiant_lire(){ - return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26); -} - -void configure_trajet(int identifiant, int couleur){ - - struct trajectoire_t trajectoire; - Trajet_config(TRAJECT_CONFIG_RAPIDE); - - switch(couleur){ - case COULEUR_JAUNE: - switch (identifiant) - { - case 0: - Localisation_set(3000-1249, 2000-63, 0); - Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, - 3000-750, 1400, 3000-750, 2100, 0, 0); - break; - case 1: - Localisation_set(3000-1249, 2000-63, 0); - Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, - 3000-750, 1400, 3000-750, 2100, 0, 0); - break; - case 2: - Localisation_set(3000-1245, 2000-63, 0); - Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, - 3000-540, 1400, 3100, 1400, M_PI, M_PI); - break; - case 3: - Localisation_set(3000-1130, 2000-63, 0); - Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, - 3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI); - break; - case 4: - Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE); - Localisation_set(3000-1364, 2000-63, 0); - Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, - 2700-900, 600, 2700-0, 0, 0, 0); - break; - case 5: - Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE); - Localisation_set(3000-1450, 2000-63, 0); - Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, - 3000-930, 970, 0, 1200, -M_PI / 2., M_PI); - break; - case 6: - break; - case 7: - break; - - default: - break; - } - break; - - case COULEUR_BLEU: - switch (identifiant) - { - case 0: - Localisation_set(1249, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, - 750, 1400, 750, 2100, M_PI, M_PI); - break; - case 1: - Localisation_set(1249, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, - 750, 1400, 750, 2100, M_PI, M_PI); - break; - case 2: - Localisation_set(1245, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, - 540, 1400, -100, 1400, M_PI, M_PI); - break; - case 3: - Localisation_set(1121, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, - 606, 2000-590, 225, 2000-225, M_PI, M_PI); - break; - case 4: - Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE); - Localisation_set(1364, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, - 900, 600, 0, 0, -M_PI / 2., M_PI); - break; - case 5: - Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE); - Localisation_set(1450, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, - 930, 970, 3000, 1200, -M_PI / 2., M_PI); - break; - case 6: - break; - case 7: - break; - - default: - break; - } - break; - } - - Trajet_debut_trajectoire(trajectoire); - + printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));*/ } \ No newline at end of file diff --git a/quadrature_encoder.pio b/quadrature_encoder.pio deleted file mode 100644 index d245d4b..0000000 --- a/quadrature_encoder.pio +++ /dev/null @@ -1,165 +0,0 @@ -; -; Copyright (c) 2021 pmarques-dev @ github -; -; SPDX-License-Identifier: BSD-3-Clause -; - -.program quadrature_encoder - -; this code must be loaded into address 0, but at 29 instructions, it probably -; wouldn't be able to share space with other programs anyway -.origin 0 - - -; the code works by running a loop that continuously shifts the 2 phase pins into -; ISR and looks at the lower 4 bits to do a computed jump to an instruction that -; does the proper "do nothing" | "increment" | "decrement" action for that pin -; state change (or no change) - -; ISR holds the last state of the 2 pins during most of the code. The Y register -; keeps the current encoder count and is incremented / decremented according to -; the steps sampled - -; writing any non zero value to the TX FIFO makes the state machine push the -; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case -; sampling loop takes 14 cycles, so this program is able to read step rates up -; to sysclk / 14 (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec) - - -; 00 state - JMP update ; read 00 - JMP decrement ; read 01 - JMP increment ; read 10 - JMP update ; read 11 - -; 01 state - JMP increment ; read 00 - JMP update ; read 01 - JMP update ; read 10 - JMP decrement ; read 11 - -; 10 state - JMP decrement ; read 00 - JMP update ; read 01 - JMP update ; read 10 - JMP increment ; read 11 - -; to reduce code size, the last 2 states are implemented in place and become the -; target for the other jumps - -; 11 state - JMP update ; read 00 - JMP increment ; read 01 -decrement: - ; note: the target of this instruction must be the next address, so that - ; the effect of the instruction does not depend on the value of Y. The - ; same is true for the "JMP X--" below. Basically "JMP Y--, " - ; is just a pure "decrement Y" instruction, with no other side effects - JMP Y--, update ; read 10 - - ; this is where the main loop starts -.wrap_target -update: - ; we start by checking the TX FIFO to see if the main code is asking for - ; the current count after the PULL noblock, OSR will have either 0 if - ; there was nothing or the value that was there - SET X, 0 - PULL noblock - - ; since there are not many free registers, and PULL is done into OSR, we - ; have to do some juggling to avoid losing the state information and - ; still place the values where we need them - MOV X, OSR - MOV OSR, ISR - - ; the main code did not ask for the count, so just go to "sample_pins" - JMP !X, sample_pins - - ; if it did ask for the count, then we push it - MOV ISR, Y ; we trash ISR, but we already have a copy in OSR - PUSH - -sample_pins: - ; we shift into ISR the last state of the 2 input pins (now in OSR) and - ; the new state of the 2 pins, thus producing the 4 bit target for the - ; computed jump into the correct action for this state - MOV ISR, NULL - IN OSR, 2 - IN PINS, 2 - MOV PC, ISR - - ; the PIO does not have a increment instruction, so to do that we do a - ; negate, decrement, negate sequence -increment: - MOV X, !Y - JMP X--, increment_cont -increment_cont: - MOV Y, !X -.wrap ; the .wrap here avoids one jump instruction and saves a cycle too - - - -% c-sdk { - -#include "hardware/clocks.h" -#include "hardware/gpio.h" - -// max_step_rate is used to lower the clock of the state machine to save power -// if the application doesn't require a very high sampling rate. Passing zero -// will set the clock to the maximum, which gives a max step rate of around -// 8.9 Msteps/sec at 125MHz - -static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) -{ - pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); - gpio_pull_up(pin); - gpio_pull_up(pin + 1); - - pio_sm_config c = quadrature_encoder_program_get_default_config(offset); - sm_config_set_in_pins(&c, pin); // for WAIT, IN - sm_config_set_jmp_pin(&c, pin); // for JMP - // shift to left, autopull disabled - sm_config_set_in_shift(&c, false, false, 32); - // don't join FIFO's - sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); - - // passing "0" as the sample frequency, - if (max_step_rate == 0) { - sm_config_set_clkdiv(&c, 1.0); - } else { - // one state machine loop takes at most 14 cycles - float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate); - sm_config_set_clkdiv(&c, div); - } - - pio_sm_init(pio, sm, offset, &c); - pio_sm_set_enabled(pio, sm, true); -} - - -// When requesting the current count we may have to wait a few cycles (average -// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple -// encoders, we may request them all in one go and then fetch them all, thus -// avoiding doing the wait multiple times. If we are reading just one encoder, -// we can use the "get_count" function to request and wait - -static inline void quadrature_encoder_request_count(PIO pio, uint sm) -{ - pio->txf[sm] = 1; -} - -static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm) -{ - while (pio_sm_is_rx_fifo_empty(pio, sm)) - tight_loop_contents(); - return pio->rxf[sm]; -} - -static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) -{ - quadrature_encoder_request_count(pio, sm); - return quadrature_encoder_fetch_count(pio, sm); -} - -%} -