diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 749f44a..f79aa0b 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -1,30 +1,20 @@ -#include "config_robot.h" -#include "QEI.h" -#include "Moteurs.h" +#include #include "Asser_Moteurs.h" +#include "Plateforme.h" -// Paramètres pour PAMI -#ifdef ROBOT_TYPE_PAMI -#define ASSERMOTEUR_GAIN_P 30000.f -#define ASSERMOTEUR_GAIN_I 3000.f -#endif +#define NB_MOTEURS 2 -// Paramètre Robot 2026 -#ifdef ROBOT_PROPULSION_2026 -#define ASSERMOTEUR_GAIN_P 150.f -#define ASSERMOTEUR_GAIN_I 1.f -#endif +float consigne_mm_s[NB_MOTEURS]; // Consigne de vitesse (en mm/s) +float commande_I[NB_MOTEURS]; // Terme integral +int asser_actif[NB_MOTEURS]; - -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); +void AsserMoteur_Init(){ + QEI_init(); Moteur_Init(); - for(unsigned int i =0; i< 2; i ++){ - commande_I[i]=0; - consigne_mm_s[i]=0; + for(unsigned int i =0; i< NB_MOTEURS; i ++){ + commande_I[i] = 0; + consigne_mm_s[i] = 0; + asser_actif[i] = 0; } } @@ -33,6 +23,7 @@ void AsserMoteur_Init(int id){ /// @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; + asser_actif[moteur] = 1; } @@ -80,27 +71,29 @@ void AsserMoteurs_stop(void){ /// @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;} + //Saturation de la commande + if(commande > 32760) {commande = 32760;} + if(commande < -32760) {commande = -32760;} - Moteur_SetVitesse(moteur, commande); + Moteur_SetVitesse(moteur, commande); + } } } \ No newline at end of file diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h index dcdfe8f..2833b0b 100644 --- a/Asser_Moteurs.h +++ b/Asser_Moteurs.h @@ -1,9 +1,10 @@ #include "Moteurs.h" +#include "QEI.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); +void AsserMoteur_Init(void); void AsserMoteurs_stop(void); \ No newline at end of file diff --git a/Asser_Position.c b/Asser_Position.c index 049a646..999339d 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -1,11 +1,8 @@ +#include "Plateforme.h" #include "Localisation.h" #include "Commande_vitesse.h" #include "math.h" -#define GAIN_P_POSITION 5 -#define GAIN_P_ORIENTATION 5 - -#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN) struct position_t position_maintien; @@ -44,8 +41,8 @@ void Asser_Position(struct position_t position_consigne){ // Asservissement - avance_mm_s = delta_avance_mm * GAIN_P_POSITION; - rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION; + avance_mm_s = delta_avance_mm * ASSER_POSITION_GAIN_P_POSITION; + rotation_radian_s = delta_orientation_radian * ASSER_POSITION_GAIN_P_ORIENTATION; /*if(delta_avance_mm < 10){ rotation_radian_s=delta_avance_mm/10 * rotation_radian_s; @@ -71,7 +68,7 @@ float Asser_Position_get_erreur_angle(){ /// @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){ + if(delta_orientation_radian > ASSER_POSITION_MAX_ERREUR_ANGLE){ return 1; } return 0; diff --git a/CMakeLists.txt b/CMakeLists.txt index 74b0113..0b8a3d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,12 +1,12 @@ project(Deplacement_Robot_differentiel C) -include(../pico_sdk_import.cmake) +pico_sdk_init() add_library(Deplacement_Robot_differentiel Asser_Position.c Asser_Moteurs.c Commande_vitesse.c - Evitement.c + Geometrie.c Moteurs.c Localisation.c QEI.c diff --git a/Commande_vitesse.c b/Commande_vitesse.c index 2050702..f2604ad 100644 --- a/Commande_vitesse.c +++ b/Commande_vitesse.c @@ -1,5 +1,5 @@ +#include "Plateforme.h" #include "Asser_Moteurs.h" -#include "Geometrie_robot.h" #include "Commande_vitesse.h" diff --git a/Deplacement.h b/Deplacement.h new file mode 100644 index 0000000..6848abd --- /dev/null +++ b/Deplacement.h @@ -0,0 +1,10 @@ +#ifndef DEPLACEMENT_H +#define DEPLACEMENT_H + + +#include "Asser_Moteurs.h" +#include "Trajet.h" + + + +#endif \ No newline at end of file diff --git a/Geometrie.c b/Geometrie.c new file mode 100644 index 0000000..1d3bcaf --- /dev/null +++ b/Geometrie.c @@ -0,0 +1,82 @@ +#include "Geometrie.h" +#include "math.h" + + +/// @brief Retourne l'angle entre -PI et +PI +/// @param angle +/// @return +float Geometrie_get_angle_normalisee(float angle){ + while(angle > M_PI){ + angle -= 2* M_PI; + } + while(angle < -M_PI){ + angle += 2* M_PI; + } + return angle; +} + +/// @brief Indique si un angle est compris entre deux angles. Les angles doivent être entre -PI et PI. +/// @param angle : angle à comparer +/// @param angle_min : début de la fourchette +/// @param angle_max : fin de la fourchette +/// @return 1 si l'angle est compris entre min et max, 0 sinon +unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max){ + if(angle_min > angle_max){ + // cas où la fourchette comprend -PI. + if( (angle > angle_min) || (angle < angle_max)){ + return 1; + } + return 0; + }else{ + // Cas normal + if( (angle > angle_min) && (angle < angle_max)){ + return 1; + } + return 0; + } +} + +/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée, +/// donne l'angle consigne pour limiter les rotations inutiles. +/// Tous les angles sont en radian +/// @param angle_depart +/// @param angle_souhaite +/// @return angle_optimal en radian +float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){ + while((angle_depart - angle_souhaite) > M_PI){ + angle_souhaite += 2* M_PI; + } + while((angle_depart - angle_souhaite) < -M_PI){ + angle_souhaite -= 2* M_PI; + } + return angle_souhaite; +} + +/// @brief Indique si les deux plages d'angle se recoupent +/// @param angle1_min Début de la première plage +/// @param angle1_max Fin de la première plage +/// @param angle2_min Début de la seconde plage +/// @param angle2_max Fin de la seconde plage +/// @return 1 si les deux plages s'intersectent, 0 sinon +unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max){ + // Pour que les plages s'intersectent, soit : + // * angle1_min est compris entre angle2_min et angle2_max + // * angle1_max est compris entre angle2_min et angle2_max + // * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant) + if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) || + Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) || + Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){ + return 1; + } + return 0; +} + +/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée. +struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){ + struct position_t position_arrivée; + position_arrivée.angle_radian = position_depart.angle_radian; + position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm; + position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm; + + return position_arrivée; +} diff --git a/Geometrie.h b/Geometrie.h new file mode 100644 index 0000000..0fd96fd --- /dev/null +++ b/Geometrie.h @@ -0,0 +1,22 @@ +#ifndef GEOMETRIE_H +#define GEOMETRIE_H + +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +#define DEGRE_EN_RADIAN (M_PI / 180.) +#define DISTANCE_INVALIDE (-1.) + +struct position_t{ + float x_mm, y_mm; + float angle_radian; +}; + +float Geometrie_get_angle_normalisee(float angle); +unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max); +unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max); +float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite); +struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm); + +#endif diff --git a/Localisation.c b/Localisation.c index e2e735e..b304d04 100644 --- a/Localisation.c +++ b/Localisation.c @@ -1,13 +1,11 @@ +#include "Plateforme.h" #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; diff --git a/Moteurs.c b/Moteurs.c index 9affc47..3f61d6e 100644 --- a/Moteurs.c +++ b/Moteurs.c @@ -1,11 +1,9 @@ -#include "config_robot.h" +#include "Plateforme.h" #include "hardware/pwm.h" +#include "hardware/gpio.h" #include "Moteurs.h" -#define MOTEUR_A 0 -#define MOTEUR_B 1 -#define MOTEUR_C 2 - +// Definition des broche pour le pilotage des moteurs #ifdef ROBOT_PROPULSION_2026 #define M1_VITESSE 2 //1A #define M1_SENS1 3 diff --git a/Moteurs.h b/Moteurs.h index a072484..1b8154b 100644 --- a/Moteurs.h +++ b/Moteurs.h @@ -1,4 +1,4 @@ -#include "pico/stdlib.h" +#include #ifndef MOTEURS_H #define MOTEURS_H diff --git a/Plateforme.h b/Plateforme.h new file mode 100644 index 0000000..15ff4c4 --- /dev/null +++ b/Plateforme.h @@ -0,0 +1,53 @@ + + +//#define ROBOT_PROPULSION_2026 +#define ROBOT_TYPE_PAMI + +#ifndef ROBOT_PROPULSION_2026 +#ifndef ROBOT_TYPE_PAMI +#error "Vous devez choisir un type de robot" +#endif +#endif + +#ifdef ROBOT_PROPULSION_2026 +#ifdef ROBOT_TYPE_PAMI +#error "Vous devez choisir un seul type de robot" +#endif +#endif + + + +// Paramètres pour PAMI +#ifdef ROBOT_TYPE_PAMI + // Asservissement des moteurs + #define ASSERMOTEUR_GAIN_P 30000.f + #define ASSERMOTEUR_GAIN_I 3000.f + // Commande en vitesse / Rotation + #define DISTANCE_ROUES_CENTRE_MM 52. + // Codeurs + #define PIN_QEI_CODEUR_A 11 + #define PIN_QEI_CODEUR_B 2 + #define IMPULSION_PAR_MM_50_1 (12.45f) + #define IMPULSION_PAR_MM_30_1 (7.47f) + #define IMPULSION_PAR_MM IMPULSION_PAR_MM_50_1 + // Asservissement en position + #define ASSER_POSITION_GAIN_P_POSITION 5 + #define ASSER_POSITION_GAIN_P_ORIENTATION 5 + #define ASSER_POSITION_MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN) +#endif + +// Paramètre Robot 2026 +#ifdef ROBOT_PROPULSION_2026 + #define ASSERMOTEUR_GAIN_P 150.f + #define ASSERMOTEUR_GAIN_I 1.f + // Commande en vitesse / Rotation + #define DISTANCE_ROUES_CENTRE_MM 103. + // Codeurs + #define PIN_QEI_CODEUR_A 14 + #define PIN_QEI_CODEUR_B 10 + #define IMPULSION_PAR_MM (7.57f) + // Asservissement en position + #define ASSER_POSITION_GAIN_P_POSITION 5 + #define ASSER_POSITION_GAIN_P_ORIENTATION 5 + #define ASSER_POSITION_MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN) +#endif \ No newline at end of file diff --git a/QEI.c b/QEI.c index 70e9f2b..6ccc776 100644 --- a/QEI.c +++ b/QEI.c @@ -1,4 +1,4 @@ -#include "config_robot.h" +#include "Plateforme.h" #include #include "pico/stdlib.h" #include "hardware/pio.h" @@ -7,21 +7,6 @@ #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) -#define IMPULSION_PAR_MM_robot_2026 (7.57f) - float impulsion_par_mm; @@ -32,7 +17,7 @@ bool QEI_est_init = false; PIO pio_QEI = pio0; -void QEI_init(int identifiant){ +void QEI_init(){ // Initialisation des 3 modules QEI // Chaque module QEI sera dans une machine à état du PIO 0 if(!QEI_est_init){ @@ -43,30 +28,17 @@ void QEI_init(int identifiant){ printf("PIO init error: offset != 0"); } // Initialisation des "machines à états" : -#ifdef ROBOT_PROPULSION_2026 - // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 10 et 11, clock div : 0 pour commencer - quadrature_encoder_program_init(pio_QEI, 1, offset, 10, 0); - // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 14 et 15, clock div : 0 pour commencer - quadrature_encoder_program_init(pio_QEI, 0, offset, 14, 0); -#else + // 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); + quadrature_encoder_program_init(pio_QEI, 0, offset, PIN_QEI_CODEUR_A, 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); -#endif + quadrature_encoder_program_init(pio_QEI, 1, offset, PIN_QEI_CODEUR_B, 0); QEI_A.value=0; QEI_B.value=0; QEI_est_init=true; } -#ifdef ROBOT_PROPULSION_2026 - impulsion_par_mm = IMPULSION_PAR_MM_robot_2026; -#else - impulsion_par_mm = IMPULSION_PAR_MM_50_1; - if(identifiant == 0 || identifiant >= 4){ - impulsion_par_mm = IMPULSION_PAR_MM_30_1; - } -#endif + impulsion_par_mm = IMPULSION_PAR_MM; } diff --git a/QEI.h b/QEI.h index e031360..3e40105 100644 --- a/QEI.h +++ b/QEI.h @@ -11,6 +11,6 @@ enum QEI_name_t{ extern struct QEI_t QEI_A, QEI_B, QEI_C; void QEI_update(void); -void QEI_init(int); +void QEI_init(); 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/Rotation.c b/Rotation.c index e2641e4..3a2ac7a 100644 --- a/Rotation.c +++ b/Rotation.c @@ -1,8 +1,8 @@ -#include "Strategie.h" +#include "Plateforme.h" +#include "Trajet.h" #include "Rotation.h" #include "Localisation.h" #include "Asser_Moteurs.h" -#include "Geometrie_robot.h" #include "math.h" float rotation_angle_cible; @@ -38,22 +38,22 @@ void rotation_set_vitesse(float vitesse_rad_s){ AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_mm_s); } } -enum etat_action_t rotation_gestion(float pas_de_temps_s){ +enum etat_trajet_t rotation_gestion(float pas_de_temps_s){ struct position_t position = Localisation_get(); rotation_vitesse_rad_s = Rotation_calcul_vitesse(position.angle_radian, pas_de_temps_s); if (rotation_angle_sens == 1 ){ rotation_set_vitesse(rotation_vitesse_rad_s); if (position.angle_radian > rotation_angle_cible){ - return ACTION_TERMINEE; + return TRAJET_TERMINE; } }else{ if (position.angle_radian < rotation_angle_cible){ - return ACTION_TERMINEE; + return TRAJET_TERMINE; } } - return ACTION_EN_COURS; + return TRAJET_EN_COURS; } diff --git a/Rotation.h b/Rotation.h index a7efa6a..b2dcb1c 100644 --- a/Rotation.h +++ b/Rotation.h @@ -1,4 +1,4 @@ extern float rotation_angle_cible, rotation_vitesse_rad_s; void rotation_init(float angle_cible); -enum etat_action_t rotation_gestion(float pas_de_temps_s); \ No newline at end of file +enum etat_trajet_t rotation_gestion(float pas_de_temps_s); \ No newline at end of file diff --git a/Temps.c b/Temps.c new file mode 100644 index 0000000..2674832 --- /dev/null +++ b/Temps.c @@ -0,0 +1,24 @@ +#include +#include "pico/stdlib.h" +#include "Temps.h" + +uint32_t temps_ms=0; +bool temps_est_init=false; +struct repeating_timer timer; + +bool Temps_increment(struct repeating_timer *t){ + temps_ms++; + return true; +} + +void Temps_init(void){ + if(!temps_est_init){ + temps_ms=0; + add_repeating_timer_ms(-1, Temps_increment, NULL, &timer); + temps_est_init = true; + } +} + +uint32_t Temps_get_temps_ms(void){ + return temps_ms; +} \ No newline at end of file diff --git a/Temps.h b/Temps.h new file mode 100644 index 0000000..0a575a3 --- /dev/null +++ b/Temps.h @@ -0,0 +1,5 @@ +#include "pico/stdlib.h" + +bool Temps_increment(struct repeating_timer *t); +void Temps_init(void); +uint32_t Temps_get_temps_ms(void); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c index ddd111f..4024e93 100644 --- a/Trajet.c +++ b/Trajet.c @@ -1,10 +1,6 @@ #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); @@ -26,8 +22,6 @@ float vitesse_max_contrainte_obstacle; /// @brief Initialise le module Trajet. A appeler en phase d'initialisation void Trajet_init(int id){ - Temps_init(); - AsserMoteur_Init(id); abscisse = 0; vitesse_mm_s = 0; position_mm = 0; diff --git a/Trajet.h b/Trajet.h index a07fcf2..abf75a6 100644 --- a/Trajet.h +++ b/Trajet.h @@ -1,9 +1,10 @@ -#include "pico/stdlib.h" -#include "Trajectoire.h" - #ifndef TRAJET_H #define TRAJET_H +#include "pico/stdlib.h" +#include "Trajectoire.h" +#include "Asser_Position.h" + enum etat_trajet_t{ TRAJET_EN_COURS, TRAJET_TERMINE diff --git a/quadrature_encoder.pio b/quadrature_encoder.pio new file mode 100644 index 0000000..d245d4b --- /dev/null +++ b/quadrature_encoder.pio @@ -0,0 +1,165 @@ +; +; 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); +} + +%} +