From 67a49c57cb1a00bb36a19231bcf6211c2d572b30 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sun, 28 Apr 2024 22:35:20 +0200
Subject: [PATCH] Asservissement en position fonctionnel

---
 Asser_Position.c | 43 +++++++++++++++++++++++++++++++++++++++++++
 Asser_Position.h |  4 ++++
 CMakeLists.txt   |  1 +
 main.c           |  8 ++++++--
 4 files changed, 54 insertions(+), 2 deletions(-)
 create mode 100644 Asser_Position.c
 create mode 100644 Asser_Position.h

diff --git a/Asser_Position.c b/Asser_Position.c
new file mode 100644
index 0000000..f6ac6bc
--- /dev/null
+++ b/Asser_Position.c
@@ -0,0 +1,43 @@
+#include "Localisation.h"
+#include "Commande_vitesse.h"
+#include "math.h"
+
+#define GAIN_P_POSITION 50
+#define GAIN_P_ORIENTATION 500
+
+struct position_t position_maintien;
+
+/// @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){
+    float delta_avance_mm;
+    float avance_mm_s, rotation_radian_s;
+    float 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_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm);
+    delta_orientation_radian = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
+
+    // Asservissement
+    avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
+    rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
+    
+    // Commande en vitesse
+    commande_vitesse(avance_mm_s, rotation_radian_s);
+
+}
+
+void Asser_Position_set_Pos_Maintien(struct position_t position){
+    position_maintien=position;
+}
+
+void Asser_Position_maintien(){
+    Asser_Position(position_maintien);
+}
\ No newline at end of file
diff --git a/Asser_Position.h b/Asser_Position.h
new file mode 100644
index 0000000..d728db3
--- /dev/null
+++ b/Asser_Position.h
@@ -0,0 +1,4 @@
+#include "Geometrie.h"
+void Asser_Position(struct position_t position_consigne);
+void Asser_Position_set_Pos_Maintien(struct position_t position);
+void Asser_Position_maintien();
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5855e70..2e67593 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
 pico_sdk_init()
 
 add_executable(Mon_Projet
+  Asser_Position.c
   Asser_Moteurs.c
   Commande_vitesse.c
   Geometrie.c
diff --git a/main.c b/main.c
index 1ec03d5..5e81c90 100644
--- a/main.c
+++ b/main.c
@@ -7,6 +7,7 @@
 #include "pico/multicore.h"
 #include "hardware/adc.h"
 #include "hardware/pwm.h"
+#include "Asser_Position.h"
 #include "Asser_Moteurs.h"
 #include "Localisation.h"
 #include "Commande_vitesse.h"
@@ -40,6 +41,8 @@ void main(void)
 	identifiant_init();
 	Localisation_init();
 	uint32_t temps_ms = Temps_get_temps_ms();
+	struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
+	float vitesse_mm_s=100;
 	
 
 	gpio_init(LED1PIN);
@@ -49,8 +52,6 @@ void main(void)
 	
 	multicore_launch_core1(affichage);
 	
-	commande_vitesse(100 , 1.);
-
 
 	while(1){
 		
@@ -58,6 +59,9 @@ void main(void)
 			temps_ms = Temps_get_temps_ms();
 			if(temps_ms % step_ms == 0){
 				QEI_update();
+				position_robot.x_mm += temps_ms * vitesse_mm_s / 1000.;
+				position_robot.y_mm += temps_ms * vitesse_mm_s / 1000.;
+				Asser_Position(position_robot);
 				AsserMoteur_Gestion(step_ms);
 				Localisation_gestion();
 			}