From 2d93697571401e842b9366522eaf69da9c2de731 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Thu, 8 Dec 2022 20:25:36 +0100
Subject: [PATCH] =?UTF-8?q?Ajout=20de=20la=20boucle=20d'asservissement=20e?=
 =?UTF-8?q?n=20position.=20Fonctionnel=20mais=20les=20gains=20ne=20sont=20?=
 =?UTF-8?q?pas=20optimis=C3=A9s.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Asser_Moteurs.c    |  6 ++-
 Asser_Position.c   | 41 ++++++++++++++++++++
 Asser_Position.h   |  1 +
 CMakeLists.txt     |  1 +
 Commande_vitesse.c |  5 ++-
 test.c             | 97 +++++++++++++++++++++++++++++++++++++++++++++-
 6 files changed, 147 insertions(+), 4 deletions(-)
 create mode 100644 Asser_Position.c
 create mode 100644 Asser_Position.h

diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c
index 5a1c5e1..3df6325 100644
--- a/Asser_Moteurs.c
+++ b/Asser_Moteurs.c
@@ -27,7 +27,9 @@ void AsserMoteur_Init(){
     }
 }
 
-
+/// @brief Défini une consigne de vitesse pour le moteur indiqué.
+/// @param  moteur : Moteur à asservir
+/// @param _consigne_mm_s : consigne de vitesse en mm/s
 void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double _consigne_mm_s){
     consigne_mm_s[moteur] = _consigne_mm_s;
 
@@ -50,6 +52,8 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
     return distance / temps;
 }
 
+/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
+/// @param step_ms 
 void AsserMoteur_Gestion(int step_ms){
     // Pour chaque moteur
     for(uint moteur=MOTEUR_A; moteur<MOTEUR_C+1; moteur++ ){
diff --git a/Asser_Position.c b/Asser_Position.c
new file mode 100644
index 0000000..8537499
--- /dev/null
+++ b/Asser_Position.c
@@ -0,0 +1,41 @@
+#include "Localisation.h"
+#include "Commande_vitesse.h"
+#include "math.h"
+
+#define GAIN_P_POSITION 100
+#define GAIN_P_ORIENTATION 100
+
+/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
+/// C'est à la consigne d'être défini avant pour être atteignable.
+/// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms);
+/// @param position_consigne : position à atteindre dans le référentiel de la table.
+void Asser_Position(struct position_t position_consigne){
+    double vitesse_x_mm_s, vitesse_y_mm_s, rotation_radian_s;
+    double vitesse_robot_x_mm_s, vitesse_robot_y_mm_s;
+    double delta_x_mm, delta_y_mm, delta_orientation_radian;
+    struct position_t position_actuelle;
+
+    position_actuelle = Localisation_get();
+
+    // Calcul de l'erreur
+    delta_x_mm = position_consigne.x_mm - position_actuelle.x_mm;
+    delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm;
+    delta_orientation_radian = position_consigne.angle_radian - position_actuelle.angle_radian;
+
+    // Asservissement
+    vitesse_x_mm_s = delta_x_mm * GAIN_P_POSITION;
+    vitesse_y_mm_s = delta_y_mm * GAIN_P_POSITION;
+    rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
+    
+    // Projection des translations dans le référentiel du robot
+    // C'est pas bon, c'est l'inverse !!!
+    //vitesse_robot_x_mm_s = cos(position_actuelle.angle_radian) * vitesse_x_mm_s - sin(position_actuelle.angle_radian) * vitesse_y_mm_s;
+    //vitesse_robot_y_mm_s = sin(position_actuelle.angle_radian) * vitesse_x_mm_s + cos(position_actuelle.angle_radian) * vitesse_y_mm_s;
+    vitesse_robot_x_mm_s = cos(position_actuelle.angle_radian) * vitesse_x_mm_s + sin(position_actuelle.angle_radian) * vitesse_y_mm_s;
+    vitesse_robot_y_mm_s = -sin(position_actuelle.angle_radian) * vitesse_x_mm_s + cos(position_actuelle.angle_radian) * vitesse_y_mm_s;
+
+    // Commande en vitesse
+    commande_vitesse(vitesse_robot_x_mm_s, vitesse_robot_y_mm_s, rotation_radian_s);
+    
+
+}
\ No newline at end of file
diff --git a/Asser_Position.h b/Asser_Position.h
new file mode 100644
index 0000000..772bdad
--- /dev/null
+++ b/Asser_Position.h
@@ -0,0 +1 @@
+void Asser_Position(struct position_t position_consigne);
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cdd460b..bf79063 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ add_executable(test
 test.c
 APDS_9960.c
 Asser_Moteurs.c
+Asser_Position.c
 Commande_vitesse.c
 QEI.c
 gyro.c
diff --git a/Commande_vitesse.c b/Commande_vitesse.c
index 1e30208..953a82b 100644
--- a/Commande_vitesse.c
+++ b/Commande_vitesse.c
@@ -1,7 +1,10 @@
 #include "Asser_Moteurs.h"
 #include "Geometrie_robot.h"
 
-
+/// @brief Commande de la vitesse dans le référentiel du robot
+/// @param vitesse_x_mm_s : Vitesse x en mm/s dans le référentiel du robot
+/// @param vitesse_y_mm_s : Vitesse y en mm/s dans le référentiel du robot
+/// @param orientation_radian_s : Rotation en radian/s dans le référentiel du robot
 void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s){
     double vitesse_roue_a, vitesse_roue_b, vitesse_roue_c;
 
diff --git a/test.c b/test.c
index bec7cd0..b6fe905 100644
--- a/test.c
+++ b/test.c
@@ -3,6 +3,7 @@
 #include "pico/stdlib.h"
 #include "hardware/gpio.h"
 #include "pico/binary_info.h"
+#include "math.h"
 
 #include "gyro.h"
 #include "Temps.h"
@@ -13,7 +14,8 @@
 #include "Asser_Moteurs.h"
 #include "Localisation.h"
 #include "Commande_vitesse.h"
-#include "math.h"
+#include "Asser_Position.h"
+
 
 const uint LED_PIN = 25;
 const uint LED_PIN_ROUGE = 28;
@@ -34,6 +36,9 @@ int test_avance(void);
 int test_cde_vitesse_rotation();
 int test_cde_vitesse_rectangle();
 int test_cde_vitesse_cercle();
+int test_asser_position_avance();
+int test_asser_position_avance_et_tourne();
+void affiche_localisation();
 
 int main() {
     bi_decl(bi_program_description("This is a test binary."));
@@ -139,6 +144,8 @@ int mode_test(){
     printf("E - Commande en vitesse - rotation pure\n");
     printf("F - Commande en vitesse - carré\n");
     printf("G - Commande en vitesse - cercle\n");
+    printf("H - Asser Position - avance\n");
+    printf("I - Asser Position - avance et tourne\n");
     printf("M - pour les moteurs\n");
     printf("L - pour la localisation\n");
     stdio_flush();
@@ -179,6 +186,16 @@ int mode_test(){
     case 'g':
         while(test_cde_vitesse_cercle());
         break;
+
+    case 'H':
+    case 'h':
+        while(test_asser_position_avance());
+        break;
+
+    case 'I':
+    case 'i':
+        while(test_asser_position_avance_et_tourne());
+        break;
         
     case 'M':
     case 'm':
@@ -205,6 +222,74 @@ int mode_test(){
     
 }
 
+int test_asser_position_avance_et_tourne(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    struct position_t position_consigne;
+
+    position_consigne.angle_radian = 0;
+    position_consigne.x_mm = 0;
+    position_consigne.y_mm = 0;
+    
+    printf("Le robot avance à 100 mm/s\n");
+    multicore_launch_core1(affiche_localisation);
+    do{
+        QEI_update();
+        Localisation_gestion();
+        AsserMoteur_Gestion(_step_ms);
+        
+        position_consigne.angle_radian = (double) temps_ms /1000. ;
+        /*
+        if(temps_ms < 10000){
+            position_consigne.y_mm = (double) temps_ms * 100. / 1000.;
+        }else if(temps_ms < 10000){ 
+            position_consigne.y_mm = 1000 - (double) temps_ms * 100. / 1000.;
+        }else{
+            temps_ms = 0;
+        }*/
+
+        position_consigne.y_mm = (double) temps_ms * 100. / 1000.;
+
+        Asser_Position(position_consigne);
+        temps_ms += _step_ms;
+        sleep_ms(_step_ms);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+}
+
+int test_asser_position_avance(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    struct position_t position;
+
+    position.angle_radian = 0;
+    position.x_mm = 0;
+    position.y_mm = 0;
+    
+    printf("Le robot avance à 100 mm/s\n");
+    do{
+        QEI_update();
+        Localisation_gestion();
+        AsserMoteur_Gestion(_step_ms);
+        
+        if(temps_ms < 5000){
+            position.x_mm = (double) temps_ms * 100. / 1000.;
+        }else if(temps_ms < 10000){ 
+            position.x_mm = 1000 - (double) temps_ms * 100. / 1000.;
+        }else{
+            temps_ms = 0;
+        }
+
+        Asser_Position(position);
+        temps_ms += _step_ms;
+        sleep_ms(_step_ms);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+}
 
 int test_cde_vitesse_rotation(){
     int lettre, _step_ms = 1;
@@ -252,7 +337,6 @@ int test_cde_vitesse_rectangle(){
 
 int test_cde_vitesse_cercle(){
     int lettre, _step_ms = 1, temps_ms=0;
-    uint64_t t_apres, t_avant;
 
     printf("déplacement en cercle du robot : 100 mm/s\n");
     do{
@@ -289,6 +373,15 @@ int test_avance(void){
     return 0;
 }
 
+void affiche_localisation(){
+    struct position_t position;
+    while(1){
+        position = Localisation_get();
+        printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+
+    }
+}
+
 void test_asser_moteur_printf(){
     int _step_ms = 1;
     while(1){