Isolement des données liées à la plateforme (sauf pour Moteur.c)

Asser_Moteurs.c

@@ -1,30 +1,20 @@
-#include "config_robot.h"
+#include <stdint.h>
-#include "QEI.h"
-#include "Moteurs.h"
 #include "Asser_Moteurs.h"
+#include "Plateforme.h"
 
-// Paramètres pour PAMI
+#define NB_MOTEURS 2
-#ifdef ROBOT_TYPE_PAMI
-#define ASSERMOTEUR_GAIN_P 30000.f
-#define ASSERMOTEUR_GAIN_I 3000.f
-#endif
 
-// Paramètre Robot 2026
+float consigne_mm_s[NB_MOTEURS]; // Consigne de vitesse (en mm/s)
-#ifdef ROBOT_PROPULSION_2026
+float commande_I[NB_MOTEURS]; // Terme integral
-#define ASSERMOTEUR_GAIN_P 150.f
+int asser_actif[NB_MOTEURS];
-#define ASSERMOTEUR_GAIN_I 1.f
-#endif
 
-
+void AsserMoteur_Init(){
-float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
+    QEI_init();
-float commande_I[3]; // Terme integral
-
-void AsserMoteur_Init(int id){
-    QEI_init(id);
     Moteur_Init();
-    for(unsigned int i =0; i< 2; i ++){
+    for(unsigned int i =0; i< NB_MOTEURS; i ++){
-        commande_I[i]=0;
+        commande_I[i] = 0;
-        consigne_mm_s[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,7 +71,8 @@ void AsserMoteurs_stop(void){
 /// @param step_ms 
 void AsserMoteur_Gestion(int step_ms){
     // Pour chaque moteur
-    for(uint moteur=MOTEUR_A; moteur<MOTEUR_B+1; moteur++ ){
+    for(enum t_moteur moteur=MOTEUR_A; moteur<NB_MOTEURS; moteur++ ){
+        if(asser_actif[moteur] == 1){
             float erreur; // Erreur entre la consigne et la vitesse actuelle
             float commande_P; // Terme proportionnel
             float commande;
@@ -101,6 +93,7 @@ void AsserMoteur_Gestion(int step_ms){
             if(commande < -32760) {commande = -32760;}
 
             Moteur_SetVitesse(moteur, commande);
+        }
 
     }
 }

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);

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;
+    avance_mm_s = delta_avance_mm * ASSER_POSITION_GAIN_P_POSITION;
-    rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
+    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;

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

Commande_vitesse.c

@@ -1,5 +1,5 @@
+#include "Plateforme.h"
 #include "Asser_Moteurs.h"
-#include "Geometrie_robot.h"
 #include "Commande_vitesse.h"
 
 

Deplacement.h

@@ -0,0 +1,10 @@
+#ifndef DEPLACEMENT_H
+#define DEPLACEMENT_H
+
+
+#include "Asser_Moteurs.h"
+#include "Trajet.h"
+
+
+
+#endif

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;
+}

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

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;

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
+// Definition des broche pour le pilotage des moteurs
-#define MOTEUR_B 1
-#define MOTEUR_C 2
-
 #ifdef ROBOT_PROPULSION_2026
     #define M1_VITESSE 2 //1A
     #define M1_SENS1 3

Moteurs.h

@@ -1,4 +1,4 @@
-#include "pico/stdlib.h"
+#include <stdint.h>
 
 #ifndef MOTEURS_H
 #define MOTEURS_H

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

QEI.c

@@ -1,4 +1,4 @@
-#include "config_robot.h"
+#include "Plateforme.h"
 #include <stdio.h>
 #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);
+        quadrature_encoder_program_init(pio_QEI, 1, offset, PIN_QEI_CODEUR_B, 0);
-#endif
 
         QEI_A.value=0;
         QEI_B.value=0;
         QEI_est_init=true;
     }
-#ifdef ROBOT_PROPULSION_2026
+    impulsion_par_mm = IMPULSION_PAR_MM;
-    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
 
 }
 

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);

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;
 
 }
 

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);
+enum etat_trajet_t rotation_gestion(float pas_de_temps_s);

Temps.c

@@ -0,0 +1,24 @@
+#include <stdio.h>
+#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;
+}

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);

Trajet.c

@@ -1,10 +1,6 @@
 #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);
@@ -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;

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

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--, <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);
+}
+
+%}
+