Ajout de l'évitement & documentation & petits ajustements

Asser_Moteurs.c

@@ -64,14 +64,17 @@ uint32_t AsserMoteur_RobotImmobile(int step_ms){
 
 void AsserMoteurs_stop(void){
     AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 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++ ){
+        // On vérifie que l'asservissement est actif pour le moteur en question
         if(asser_actif[moteur] == 1){
             float erreur; // Erreur entre la consigne et la vitesse actuelle
             float commande_P; // Terme proportionnel

Asser_Position.c

@@ -1,7 +1,6 @@
 #include "Plateforme.h"
-#include "Localisation.h"
-#include "Commande_vitesse.h"
 #include "math.h"
+#include "Asser_Position.h"
 
 
 struct position_t position_maintien;

Asser_Position.h

@@ -1,4 +1,6 @@
 #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

@@ -5,7 +5,9 @@ pico_sdk_init()
 add_library(Deplacement_Robot_differentiel
   Asser_Position.c
   Asser_Moteurs.c
-  Commande_vitesse.c
+  Deplacement.c
+  Evitement.c
+  Loi_de_commande.c
   Geometrie.c
   Moteurs.c
   Localisation.c

Deplacement.c

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

@@ -5,6 +5,16 @@
 #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

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

@@ -0,0 +1,8 @@
+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);

Loi_de_commande.c

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

Readme.md

@@ -1,9 +1,25 @@
-Submodule Servomoteurs pour le RP2040
+Submodule Déplacement 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
 ===========
 
@@ -23,13 +39,13 @@ Configuration de la compilation
 
 Dans le fichier CMakeLists.txt, ajouter le dossier du sub module:
 
-    add_subdirectory(RP2040_Servomoteurs)
+    add_subdirectory(Module_deplacement_robot_differentiel)
 
 Dans le fichier CMakeLists.txt, ajouter la bibliothèque RP2040_Servomoteur :
 
     target_link_libraries(Modele_RPiPico
       ...
-      RP2040_Servomoteur
+      Deplacement_Robot_differentiel
     )
 
 Intégration au code source
@@ -37,19 +53,22 @@ Intégration au code source
 
 Ajout du fichier d'include :
 
-    #include "RP2040_Servomoteurs/Servomoteur.h"
+    #include "Module_deplacement_robot_differentiel/Deplacement.h"
 
 Initilisation du module :
 
-    Servomoteur_Init();
+    AsserMoteur_Init();
+    Trajet_init();
 
-Envoie d'une consigne au servomoteur :
+Fonctions cycliques :
 
-    Servomoteur_set(num_gpio, position);
+    AsserMoteur_Gestion(step_ms);
+    Localisation_gestion();
 
 Avec :
-- 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.
+- step_ms : le temps entre chaque appel de __AsserMoteur_Gestion()__
+
+Vous avez deux exemples de code plus bas.
 
 Cas où vous clonez un projet contenant des submodules
 -----------------------------------------------------
@@ -63,3 +82,66 @@ 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);
+        }
+        
+    }

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, float rayon, float orientation_debut_rad, float orientation_fin_rad){
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad,
+                            float rayon){
     trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
     trajectoire->p1.x = centre_x;
     trajectoire->p1.y = centre_y;
@@ -19,24 +19,19 @@ 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,
-                        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){
     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){
+                        float p4_x, float p4_y){
     trajectoire->type = TRAJECTOIRE_BEZIER;
     trajectoire->p1.x = p1_x;
     trajectoire->p1.y = p1_y;
@@ -47,8 +42,6 @@ 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,11 +35,9 @@ 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 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);
+                            float rayon);
+void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_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);
 void Trajectoire_composee_init(struct trajectoire_t * trajectoire);
 void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire);
 

Trajet.c

@@ -14,14 +14,14 @@ const float acceleration_mm_ss_obstacle = 500;
 struct trajectoire_t trajet_trajectoire;
 struct position_t position_consigne;
 
-float distance_obstacle_mm;
+float trajet_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(int id){
+void Trajet_init(){
     abscisse = 0;
     vitesse_mm_s = 0;
     position_mm = 0;
@@ -158,11 +158,11 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
 
 
 float Trajet_get_obstacle_mm(void){
-    return distance_obstacle_mm;
+    return trajet_distance_obstacle_mm;
 }
 
 void Trajet_set_obstacle_mm(float distance_mm){
-    distance_obstacle_mm = distance_mm;
+    trajet_distance_obstacle_mm = distance_mm;
 }
 
 

Trajet.h

@@ -7,7 +7,8 @@
 
 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²)
@@ -22,7 +23,7 @@ enum etat_trajet_t{
 
 extern const float distance_pas_obstacle;
 
-void Trajet_init(int);
+void Trajet_init(void);
 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);