Passage sur Xiao RP2040
This commit is contained in:
parent
7538fdd311
commit
8dfe505a44
1
.vscode/c_cpp_properties.json
vendored
1
.vscode/c_cpp_properties.json
vendored
@ -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",
|
||||
|
||||
@ -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<MOTEUR_B+1; moteur++ ){
|
||||
float erreur; // Erreur entre la consigne et la vitesse actuelle
|
||||
float commande_P; // Terme proportionnel
|
||||
float commande;
|
||||
|
||||
// Calcul de l'erreur
|
||||
erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
|
||||
|
||||
// Calcul du terme propotionnel
|
||||
commande_P = erreur * ASSERMOTEUR_GAIN_P;
|
||||
|
||||
// Calcul du terme integral
|
||||
commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
|
||||
|
||||
commande = commande_P + commande_I[moteur];
|
||||
|
||||
//Saturation de la commande
|
||||
if(commande > 32760) {commande = 32760;}
|
||||
if(commande < -32760) {commande = -32760;}
|
||||
|
||||
Moteur_SetVitesse(moteur, commande);
|
||||
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
@ -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;
|
||||
}
|
||||
@ -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();
|
||||
@ -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_spi
|
||||
hardware_i2c
|
||||
hardware_pwm
|
||||
hardware_pio
|
||||
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
|
||||
)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
@ -1,2 +0,0 @@
|
||||
void commande_vitesse(float vitesse_avance_mm_s, float orientation_radian_s);
|
||||
void commande_vitesse_stop(void);
|
||||
@ -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;
|
||||
}
|
||||
@ -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);
|
||||
98
Moteurs.c
98
Moteurs.c
@ -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);
|
||||
}
|
||||
14
Moteurs.h
14
Moteurs.h
@ -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);
|
||||
@ -13,101 +13,56 @@
|
||||
|
||||
#include "platform.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/i2c.h"
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include <stdio.h>
|
||||
#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
|
||||
void SPI_beginTransaction(char my_ss_pin){
|
||||
/* switch(my_ss_pin){
|
||||
case D0: SPI.begin(SCK, MISO, MOSI, SS); break;
|
||||
case D1: SPI.begin(SCK, D3, MOSI, SS); break;
|
||||
}*/
|
||||
//
|
||||
gpio_put(my_ss_pin, 0);
|
||||
|
||||
#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<count; i++){
|
||||
buffer[2+i] = values[i];
|
||||
}
|
||||
|
||||
// Define timeout - now + 1s.
|
||||
timeout_time = time_us_64() + 5000000;
|
||||
statu = i2c_write_blocking_until (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0, timeout_time);
|
||||
//statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0);
|
||||
if(statu == PICO_ERROR_GENERIC){
|
||||
printf("I2C - Write - Envoi registre Echec %x\n", adresse_7_bits);
|
||||
return I2C_FAILED;
|
||||
}else if(statu == PICO_ERROR_TIMEOUT){
|
||||
printf("Erreur ecrire registre: timeout\n");
|
||||
printf("Adresse : %x\n",adresse_7_bits << 1 );
|
||||
return I2C_FAILED;
|
||||
}
|
||||
|
||||
return I2C_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
/// @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_read_register(char adresse_7_bits, uint16_t index, uint8_t *pdata, uint32_t count){
|
||||
int statu;
|
||||
uint8_t buffer[MAX_I2C_BUFFER];
|
||||
uint8_t index_to_unint8[2];
|
||||
|
||||
index_to_unint8[0] = (index >> 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;
|
||||
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;
|
||||
|
||||
return i2c_write_register(p_platform->address >> 1, RegisterAdress, p_values, size);
|
||||
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;
|
||||
uint32_t index =0;
|
||||
uint32_t buffer_size = 32;
|
||||
uint8_t my_ss_pin;
|
||||
uint8_t tampon[2];
|
||||
my_ss_pin = p_platform->address;
|
||||
|
||||
return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_values, size);
|
||||
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;
|
||||
}
|
||||
101
QEI.c
101
QEI.c
@ -1,101 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#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;
|
||||
}
|
||||
16
QEI.h
16
QEI.h
@ -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);
|
||||
167
Trajectoire.c
167
Trajectoire.c
@ -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));
|
||||
|
||||
}
|
||||
@ -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
|
||||
@ -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;
|
||||
}
|
||||
@ -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);
|
||||
@ -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;
|
||||
}
|
||||
@ -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);
|
||||
@ -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;
|
||||
}
|
||||
@ -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);
|
||||
220
Trajet.c
220
Trajet.c
@ -1,220 +0,0 @@
|
||||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
37
Trajet.h
37
Trajet.h
@ -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
|
||||
@ -11,6 +11,7 @@
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#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));
|
||||
|
||||
@ -1,6 +1,9 @@
|
||||
#include "vl53l8cx_api.h"
|
||||
#include "Trajet.h"
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#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(){
|
||||
|
||||
@ -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);
|
||||
347
main.c
347
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 <stdio.h>
|
||||
#include <math.h>
|
||||
#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);
|
||||
|
||||
// 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;
|
||||
|
||||
//multicore_launch_core1(gestion_affichage);
|
||||
multicore_launch_core1(gestion_VL53L8CX);
|
||||
sleep_ms(5000);
|
||||
printf("Demarrage...\n");
|
||||
|
||||
//multicore_launch_core1(gestion_affichage);
|
||||
|
||||
|
||||
configure_trajet(identifiant_lire(), get_couleur());
|
||||
gestion_VL53L8CX();
|
||||
|
||||
|
||||
float vitesse_init =300;
|
||||
vitesse = vitesse_init;
|
||||
|
||||
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
|
||||
}
|
||||
|
||||
while(get_tirette(identifiant_lire()));
|
||||
gpio_put(LED1PIN, 0);
|
||||
|
||||
// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
|
||||
if(identifiant_lire() == 3){
|
||||
sleep_ms(90000);
|
||||
uint8_t capteur_init(VL53L8CX_Configuration * capteur){
|
||||
uint8_t status=0;
|
||||
printf("debut init\n");
|
||||
//pinMode(capteur->platform.address, OUTPUT);
|
||||
|
||||
status = vl53l8cx_init(capteur);
|
||||
if(status != 0){
|
||||
while(1){
|
||||
printf("Status init = %d\n", status);
|
||||
WaitMs(&(capteur->platform), 1000);
|
||||
break;
|
||||
}
|
||||
}
|
||||
sleep_ms(100);
|
||||
|
||||
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()));*/
|
||||
}
|
||||
@ -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--, <next addr>"
|
||||
; 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);
|
||||
}
|
||||
|
||||
%}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user