Asser_Moteurs.c

@@ -1,20 +1,30 @@
-#include <stdint.h>
+#include "config_robot.h"
+#include "QEI.h"
+#include "Moteurs.h"
 #include "Asser_Moteurs.h"
-#include "Plateforme.h"
 
-#define NB_MOTEURS 2
+// Paramètres pour PAMI
+#ifdef ROBOT_TYPE_PAMI
+#define ASSERMOTEUR_GAIN_P 30000.f
+#define ASSERMOTEUR_GAIN_I 3000.f
+#endif
 
-float consigne_mm_s[NB_MOTEURS]; // Consigne de vitesse (en mm/s)
+// Paramètre Robot 2026
-float commande_I[NB_MOTEURS]; // Terme integral
+#ifdef ROBOT_PROPULSION_2026
-int asser_actif[NB_MOTEURS];
+#define ASSERMOTEUR_GAIN_P 150.f
+#define ASSERMOTEUR_GAIN_I 1.f
+#endif
 
-void AsserMoteur_Init(){
+
-    QEI_init();
+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< NB_MOTEURS; i ++){
+    for(unsigned int i =0; i< 2; i ++){
-        commande_I[i] = 0;
+        commande_I[i]=0;
-        consigne_mm_s[i] = 0;
+        consigne_mm_s[i]=0;
-        asser_actif[i] = 0;
     }
 }
 
@@ -23,7 +33,6 @@ void AsserMoteur_Init(){
 /// @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;
 
 }
 
@@ -64,39 +73,34 @@ uint32_t AsserMoteur_RobotImmobile(int step_ms){
 
 void AsserMoteurs_stop(void){
     AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
 }
 
 /// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
 /// @param step_ms 
 void AsserMoteur_Gestion(int step_ms){
-    // On actualise les codeurs
-    QEI_update();
     // Pour chaque moteur
-    for(enum t_moteur moteur=MOTEUR_A; moteur<NB_MOTEURS; moteur++ ){
+    for(uint moteur=MOTEUR_A; moteur<MOTEUR_B+1; moteur++ ){
-        // On vérifie que l'asservissement est actif pour le moteur en question
+        float erreur; // Erreur entre la consigne et la vitesse actuelle
-        if(asser_actif[moteur] == 1){
+        float commande_P; // Terme proportionnel
-            float erreur; // Erreur entre la consigne et la vitesse actuelle
+        float commande;
-            float commande_P; // Terme proportionnel
-            float commande;
         
-            // Calcul de l'erreur
+        // Calcul de l'erreur
-            erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
+        erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
 
-            // Calcul du terme propotionnel
+        // Calcul du terme propotionnel
-            commande_P = erreur * ASSERMOTEUR_GAIN_P;
+        commande_P = erreur * ASSERMOTEUR_GAIN_P;
 
-            // Calcul du terme integral
+        // Calcul du terme integral
-            commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
+        commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
 
-            commande = commande_P + commande_I[moteur];
+        commande = commande_P + commande_I[moteur];
 
-            //Saturation de la commande
+        //Saturation de la commande
-            if(commande > 32760) {commande = 32760;}
+        if(commande > 32760) {commande = 32760;}
-            if(commande < -32760) {commande = -32760;}
+        if(commande < -32760) {commande = -32760;}
 
-            Moteur_SetVitesse(moteur, commande);
+        Moteur_SetVitesse(moteur, commande);
-        }
 
     }
 }

Asser_Moteurs.h

@@ -1,10 +1,9 @@
 #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(void);
+void AsserMoteur_Init(int);
 void AsserMoteurs_stop(void);

Asser_Position.c

@@ -1,7 +1,11 @@
-#include "Plateforme.h"
+#include "Localisation.h"
+#include "Commande_vitesse.h"
 #include "math.h"
-#include "Asser_Position.h"
 
+#define GAIN_P_POSITION 5
+#define GAIN_P_ORIENTATION 5
+
+#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
 
 struct position_t position_maintien;
 
@@ -40,8 +44,8 @@ void Asser_Position(struct position_t position_consigne){
 
 
     // Asservissement
-    avance_mm_s = delta_avance_mm * ASSER_POSITION_GAIN_P_POSITION;
+    avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
-    rotation_radian_s = delta_orientation_radian * ASSER_POSITION_GAIN_P_ORIENTATION;
+    rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
 
     /*if(delta_avance_mm < 10){
         rotation_radian_s=delta_avance_mm/10 * rotation_radian_s;
@@ -67,7 +71,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 > ASSER_POSITION_MAX_ERREUR_ANGLE){
+    if(delta_orientation_radian > MAX_ERREUR_ANGLE){
         return 1;
     }
     return 0;

Asser_Position.h

@@ -1,6 +1,4 @@
 #include "Geometrie.h"
-#include "Localisation.h"
-#include "Loi_de_commande.h"
 void Asser_Position(struct position_t position_consigne);
 void Asser_Position_set_Pos_Maintien(struct position_t position);
 void Asser_Position_maintien();

CMakeLists.txt

@@ -1,14 +1,12 @@
 project(Deplacement_Robot_differentiel C)
 
-pico_sdk_init()
+include(../pico_sdk_import.cmake)
 
 add_library(Deplacement_Robot_differentiel
   Asser_Position.c
   Asser_Moteurs.c
-  Deplacement.c
+  Commande_vitesse.c
   Evitement.c
-  Loi_de_commande.c
-  Geometrie.c
   Moteurs.c
   Localisation.c
   QEI.c

Commande_vitesse.c

@@ -1,6 +1,6 @@
-#include "Plateforme.h"
 #include "Asser_Moteurs.h"
-#include "Loi_de_commande.h"
+#include "Geometrie_robot.h"
+#include "Commande_vitesse.h"
 
 
 float avance_mm_s, orientation_radian_s;

Deplacement.c

@@ -1,83 +0,0 @@
-#include "Deplacement.h"
-#include "Trajet.h"
-#include "Evitement.h"
-#include "Geometrie.h"
-
-float distance_obstacle_mm;
-
-static enum {
-        PARCOURS_INIT,
-        PARCOURS_AVANCE,
-    } etat_parcourt=PARCOURS_INIT;
-
-/// @brief Reçoit la distance de l'obstacle et l'envoi à la fonction trajet
-/// si nécessaire
-/// @param _distance_mm 
-void Deplacement_set_distance_obstacle(float _distance_mm){
-    distance_obstacle_mm = _distance_mm;
-}
-
-void Deplacement_interrompre_trajet(void){
-    etat_parcourt=PARCOURS_INIT;
-}
-
-enum etat_trajet_t Deplacement_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement){
-    enum etat_trajet_t etat_action = TRAJET_EN_COURS;
-    enum etat_trajet_t etat_trajet;
-    float angle_avancement;
-    static bool trajet_inverse = false;
-    
-    
-
-    switch (etat_parcourt){
-        case PARCOURS_INIT:
-            Trajet_debut_trajectoire(trajectoire);
-            etat_parcourt = PARCOURS_AVANCE;
-            break;
-        
-        case PARCOURS_AVANCE:
-            if(evitement != EVITEMENT_SANS_EVITEMENT){
-                angle_avancement = Trajet_get_orientation_avance();
-                Trajet_set_obstacle_mm(distance_obstacle_mm);
-            }
-
-            if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
-                switch(evitement){
-                    case EVITEMENT_SANS_EVITEMENT:
-                        //printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n");
-                        break;
-                    case EVITEMENT_PAUSE_DEVANT_OBSTACLE:
-                        // Rien à faire ici
-                        break;
-                    
-                    case EVITEMENT_ARRET_DEVANT_OBSTACLE:
-                        etat_parcourt = PARCOURS_INIT;
-                        return TRAJET_ECHEC;
-
-                    case EVITEMENT_RETOUR_SI_OBSTABLE:
-                        trajet_inverse = !trajet_inverse;
-                        Trajet_inverse();
-                        break;
-
-                    case EVITEMENT_CONTOURNEMENT: // TODO
-                        break;
-                        
-                }
-
-            }
-
-            etat_trajet = Trajet_avance(step_ms/1000.);
-            if(etat_trajet == TRAJET_TERMINE){
-                if(trajet_inverse){
-                    etat_action = TRAJET_ECHEC;
-                }else{
-                    etat_action = TRAJET_TERMINE;
-                }
-                etat_parcourt = PARCOURS_INIT;
-            }
-            break;
-    }
-
-    return etat_action;
-
-}

Deplacement.h

@@ -1,20 +0,0 @@
-#ifndef DEPLACEMENT_H
-#define DEPLACEMENT_H
-
-
-#include "Asser_Moteurs.h"
-#include "Trajet.h"
-
-enum evitement_t{
-    EVITEMENT_SANS_EVITEMENT,
-    EVITEMENT_PAUSE_DEVANT_OBSTACLE,
-    EVITEMENT_ARRET_DEVANT_OBSTACLE,
-    EVITEMENT_RETOUR_SI_OBSTABLE,
-    EVITEMENT_CONTOURNEMENT
-};
-
-void Deplacement_set_distance_obstacle(float _distance_mm);
-void Deplacement_interrompre_trajet(void);
-enum etat_trajet_t Deplacement_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
-
-#endif

Evitement.c

@@ -1,38 +0,0 @@
-#include "pico/stdlib.h"
-#include "Asser_Moteurs.h"
-#include "Evitement.h"
-#include "Trajet.h"
-
-// 250 ms
-#define TEMPS_VALIDE_OBSTACLE_US 250000
-
-enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
-
-void Evitement_gestion(int step_ms){
-    static uint32_t temps_obstacle;
-    switch(evitement_statu){
-        case PAS_D_OBSTACLE:
-            if(Trajet_get_bloque() == 1 && AsserMoteur_RobotImmobile(step_ms)){
-                evitement_statu = OBSTACLE_NON_CONFIRME;
-                temps_obstacle = time_us_32();
-            }
-            break;
-        case OBSTACLE_NON_CONFIRME:
-            if(time_us_32() - temps_obstacle >  TEMPS_VALIDE_OBSTACLE_US){
-                evitement_statu = OBSTACLE_CONFIRME;
-            }
-            if(!Trajet_get_bloque()){
-                evitement_statu = PAS_D_OBSTACLE;
-            }
-            break;
-        case OBSTACLE_CONFIRME:
-            if(!Trajet_get_bloque()){
-                evitement_statu = PAS_D_OBSTACLE;
-            }
-            break;
-    }
-}
-
-enum evitement_statu_t Evitement_get_statu(){
-    return evitement_statu;
-}

Evitement.h

@@ -1,8 +0,0 @@
-enum evitement_statu_t{
-    PAS_D_OBSTACLE,
-    OBSTACLE_NON_CONFIRME,
-    OBSTACLE_CONFIRME,
-};
-
-enum evitement_statu_t Evitement_get_statu();
-void Evitement_gestion(int step_ms);

Geometrie.c

@@ -1,82 +0,0 @@
-#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

@@ -1,22 +0,0 @@
-#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,11 +1,13 @@
-#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,9 +1,11 @@
-#include "Plateforme.h"
+#include "config_robot.h"
 #include "hardware/pwm.h"
-#include "hardware/gpio.h"
 #include "Moteurs.h"
 
-// Definition des broche pour le pilotage des moteurs
+#define MOTEUR_A 0
+#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 <stdint.h>
+#include "pico/stdlib.h"
 
 #ifndef MOTEURS_H
 #define MOTEURS_H

Plateforme.h

@@ -1,53 +0,0 @@
-
-
-//#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 "Plateforme.h"
+#include "config_robot.h"
 #include <stdio.h>
 #include "pico/stdlib.h"
 #include "hardware/pio.h"
@@ -7,6 +7,21 @@
 #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;
 
 
@@ -17,7 +32,7 @@ bool QEI_est_init = false;
 PIO pio_QEI = pio0;
 
 
-void QEI_init(){
+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){
@@ -28,17 +43,30 @@ void QEI_init(){
             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, PIN_QEI_CODEUR_A, 0);
+        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, PIN_QEI_CODEUR_B, 0);
+        quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0);
+#endif
 
         QEI_A.value=0;
         QEI_B.value=0;
         QEI_est_init=true;
     }
-    impulsion_par_mm = IMPULSION_PAR_MM;
+#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
 
 }
 

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();
+void QEI_init(int);
 int QEI_get(enum QEI_name_t qei);
 float QEI_get_mm(enum QEI_name_t qei);

Readme.md

@@ -1,25 +1,9 @@
-Submodule Déplacement pour le RP2040
+Submodule Servomoteurs pour le RP2040
 =====================================
 
 Ceci est un submodule git pour intégrer facilement le support des servomoteurs à un projet.
 On crée ce Submodule en se servant de la documentation disponible ici : https://git-scm.com/book/en/v2/Git-Tools-Submodules
 
-Ce module est composé de deux parties :
-- L'asservissement des moteurs :
-  - Moteur PWM
-  - Lecture codeur
-  - asservissement
-- La gestion des trajets :
-  - Loi de commande
-  - Asservissement en position
-  - Trajectoires
-  - Trajet (accélération et décélération sur trajectoire)
-  - Déplacement avec évitement sommaire:
-    - Ignore l'obstacle
-    - Arrêt devant obstacle
-    - Pause devant obstacle
-    - Inversion trajectoire
-
 Utilisation
 ===========
 
@@ -39,13 +23,13 @@ Configuration de la compilation
 
 Dans le fichier CMakeLists.txt, ajouter le dossier du sub module:
 
-    add_subdirectory(Module_deplacement_robot_differentiel)
+    add_subdirectory(RP2040_Servomoteurs)
 
 Dans le fichier CMakeLists.txt, ajouter la bibliothèque RP2040_Servomoteur :
 
     target_link_libraries(Modele_RPiPico
       ...
-      Deplacement_Robot_differentiel
+      RP2040_Servomoteur
     )
 
 Intégration au code source
@@ -53,22 +37,19 @@ Intégration au code source
 
 Ajout du fichier d'include :
 
-    #include "Module_deplacement_robot_differentiel/Deplacement.h"
+    #include "RP2040_Servomoteurs/Servomoteur.h"
 
 Initilisation du module :
 
-    AsserMoteur_Init();
+    Servomoteur_Init();
-    Trajet_init();
 
-Fonctions cycliques :
+Envoie d'une consigne au servomoteur :
 
-    AsserMoteur_Gestion(step_ms);
+    Servomoteur_set(num_gpio, position);
-    Localisation_gestion();
 
 Avec :
-- step_ms : le temps entre chaque appel de __AsserMoteur_Gestion()__
+- num_gpio : le numéro de la GPIO du RP2040
-
+- position : la position du servomoteur, qui correspond à un temps haut du signal compris généralement en 0,5 et 2,5 ms (en fonction des servomoteurs). le fichier _Servomoteur.h_ contient des valeurs d'exemple.
-Vous avez deux exemples de code plus bas.
 
 Cas où vous clonez un projet contenant des submodules
 -----------------------------------------------------
@@ -82,66 +63,3 @@ Après avoir cloné le projet, initilisez les submodules:
 Pour récupérer les dernières mise à jour des sub-modules :
 
     git submodule update --remote
-
-Code minimale 1
-===============
-
-Ce code permet d'utiliser les fonctions d'asservissement en vitesse des moteurs
-
-    #include "Module_deplacement_robot_differentiel/Deplacement.h"
-    #include <stdio.h>
-
-    void main(void){
-        // Initilisation
-        AsserMoteur_Init();
-        // Consignes
-        AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
-        AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
-        int step_ms = 1;
-        while(1){
-            sleep_ms(step_ms);
-            // Tâche cyclique
-            AsserMoteur_Gestion(step_ms);
-        }
-    }
-
-Tandis que ce code permet d'utiliser les fonctions trajets / trajectoire avec différents mode d'évitement
-
-    #include "Module_deplacement_robot_differentiel/Deplacement.h"
-    #include <stdio.h>
-
-    void main(void)
-    {
-        stdio_init_all();
-
-        // Initialisation
-        AsserMoteur_Init();
-        Trajet_init();
-        
-        // Pour l'exemple
-        int step_ms = 1;
-        enum etat_trajet_t etat_trajet = TRAJET_EN_COURS;
-        struct trajectoire_t ma_trajectoire;
-        Trajectoire_bezier(&ma_trajectoire, 
-            0, 0,
-            250, 0,
-            0, 250,
-            250, 250);
-        
-        while(1){
-            // Tâches cycliques
-            AsserMoteur_Gestion(step_ms);
-            Localisation_gestion();
-
-            // Gestion du trajet
-            if(etat_trajet == TRAJET_EN_COURS){
-                etat_trajet = Deplacement_parcourir_trajet(ma_trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
-            }else{
-                AsserMoteurs_stop();
-            }
-
-            // Attente de 1 ms
-            sleep_ms(step_ms);
-        }
-        
-    }

Rotation.c

@@ -1,8 +1,8 @@
-#include "Plateforme.h"
+#include "Strategie.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_trajet_t rotation_gestion(float pas_de_temps_s){
+enum etat_action_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 TRAJET_TERMINE;
+            return ACTION_TERMINEE;
         }
 
     }else{
         if (position.angle_radian <  rotation_angle_cible){
-            return TRAJET_TERMINE;
+            return ACTION_TERMINEE;
         }
     }
-    return TRAJET_EN_COURS;
+    return ACTION_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_trajet_t rotation_gestion(float pas_de_temps_s);
+enum etat_action_t rotation_gestion(float pas_de_temps_s);

Temps.c

@@ -1,24 +0,0 @@
-#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

@@ -1,5 +0,0 @@
-#include "pico/stdlib.h"
-
-bool Temps_increment(struct repeating_timer *t);
-void Temps_init(void);
-uint32_t Temps_get_temps_ms(void);

Trajectoire.c

@@ -10,8 +10,8 @@
 #define NB_MAX_ITERATIONS 3
 
 
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad,
-                            float rayon){
+                        float angle_fin_rad, float rayon, float orientation_debut_rad, float orientation_fin_rad){
     trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
     trajectoire->p1.x = centre_x;
     trajectoire->p1.y = centre_y;
@@ -19,19 +19,24 @@ void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x,
     trajectoire->angle_fin_rad = angle_fin_rad;
     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){
+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 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;
@@ -42,6 +47,8 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1
     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;
 }
 
 /// @brief Initialise une trajectoire composée

Trajectoire.h

@@ -35,9 +35,11 @@ float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float
 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_rad, float angle_fin_rad,
-                            float rayon);
+                            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);
+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);
+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_composee_init(struct trajectoire_t * trajectoire);
 void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire);
 

Trajet.c

@@ -1,6 +1,10 @@
 #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);
@@ -14,14 +18,16 @@ const float acceleration_mm_ss_obstacle = 500;
 struct trajectoire_t trajet_trajectoire;
 struct position_t position_consigne;
 
-float trajet_distance_obstacle_mm;
+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'initialisation
-void Trajet_init(){
+void Trajet_init(int id){
+    Temps_init();
+    AsserMoteur_Init(id);
     abscisse = 0;
     vitesse_mm_s = 0;
     position_mm = 0;
@@ -158,11 +164,11 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
 
 
 float Trajet_get_obstacle_mm(void){
-    return trajet_distance_obstacle_mm;
+    return distance_obstacle_mm;
 }
 
 void Trajet_set_obstacle_mm(float distance_mm){
-    trajet_distance_obstacle_mm = distance_mm;
+    distance_obstacle_mm = distance_mm;
 }
 
 

Trajet.h

@@ -1,14 +1,12 @@
+#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,
+    TRAJET_TERMINE
-    TRAJET_ECHEC
 };
 
 // Vitesse et acceleration pour translation pure (en mm/s et mm/s²)
@@ -23,7 +21,7 @@ enum etat_trajet_t{
 
 extern const float distance_pas_obstacle;
 
-void Trajet_init(void);
+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);

quadrature_encoder.pio

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