From d09ee29825dbd541a97d1747f04e4c3a49916066 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Fri, 3 Mar 2023 19:50:44 +0100
Subject: [PATCH] =?UTF-8?q?D=C3=A9but=20de=20l'algo=20pour=20attraper=20le?=
 =?UTF-8?q?s=20cerises=20-=20ajout=20de=20la=20fonction=20de=20rotation=20?=
 =?UTF-8?q?autour=20d'un=20point.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Commande_vitesse.c        | 18 ++++++++++++-
 Commande_vitesse.h        |  3 ++-
 Geometrie_robot.h         |  1 +
 Strategie.h               |  4 +++
 Strategie_prise_cerises.c | 54 +++++++++++++++++++++++++++++++++++++++
 Strategie_prise_cerises.h |  1 +
 6 files changed, 79 insertions(+), 2 deletions(-)
 create mode 100644 Strategie.h
 create mode 100644 Strategie_prise_cerises.c
 create mode 100644 Strategie_prise_cerises.h

diff --git a/Commande_vitesse.c b/Commande_vitesse.c
index 159ec38..2a28bd7 100644
--- a/Commande_vitesse.c
+++ b/Commande_vitesse.c
@@ -1,8 +1,24 @@
 #include "Asser_Moteurs.h"
 #include "Geometrie_robot.h"
+#include "Commande_vitesse.h"
+
+/// @brief Commande une rotation autour d'un point dans le référentiel du robot.
+/// Cette fonction déplace le torseur cinétique du centre de rotation vers le centre du robot
+/// pour obtenir la vitesse de ce dernier.
+/// @param rotation_rad_s : Rotation en rad/s
+/// @param centre_x : centre de rotation (coordonnée X)
+/// @param centre_y : centre de rotation (coordonnée Y)
+void commande_rotation(double rotation_rad_s, double centre_x, double centre_y){
+    double vitesse_x_mm_s, vitesse_y_mm_s;
+    vitesse_x_mm_s = centre_y * rotation_rad_s;
+    vitesse_y_mm_s = -centre_x * rotation_rad_s;
+
+    commande_vitesse(vitesse_x_mm_s, vitesse_y_mm_s, rotation_rad_s);
+}
+
 
 /// @brief Commande de la vitesse dans le référentiel du robot
-/// Tel que décrit ici : http://poivron-robotique.fr/Robot-holonome-lois-de-commande.html
+/// tel que décrit ici : http://poivron-robotique.fr/Robot-holonome-lois-de-commande.html
 /// @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
diff --git a/Commande_vitesse.h b/Commande_vitesse.h
index a30747b..61d0cdb 100644
--- a/Commande_vitesse.h
+++ b/Commande_vitesse.h
@@ -1 +1,2 @@
-void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s);
\ No newline at end of file
+void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s);
+void commande_rotation(double rotation_rad_s, double centre_x, double centre_y);
\ No newline at end of file
diff --git a/Geometrie_robot.h b/Geometrie_robot.h
index 03a31c7..180f25e 100644
--- a/Geometrie_robot.h
+++ b/Geometrie_robot.h
@@ -1,2 +1,3 @@
 #define DISTANCE_ROUES_CENTRE_MM 84.25
+#define RAYON_ROBOT 125
 #define RACINE_DE_3 1.73205081
\ No newline at end of file
diff --git a/Strategie.h b/Strategie.h
new file mode 100644
index 0000000..2c399f4
--- /dev/null
+++ b/Strategie.h
@@ -0,0 +1,4 @@
+enum etat_action_t{
+    ACTION_EN_COURS,
+    ACTION_TERMINEE
+};
\ No newline at end of file
diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c
new file mode 100644
index 0000000..166eca3
--- /dev/null
+++ b/Strategie_prise_cerises.c
@@ -0,0 +1,54 @@
+#include "Strategie_prise_cerises.h"
+#include "Commande_vitesse.h"
+#include "Geometrie.h"
+#include "Geometrie_robot.h"
+#include "math.h"
+
+// Rotation en rad/s pour accoster les cerises
+#define ROTATION_CERISE 0.5f
+
+double vitesse_accostage_mm_s=100;
+
+enum etat_action_t cerise_accostage(void){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    struct position_t contacteur_gauche, contacteur_droit;
+    double rotation;
+
+    static enum {
+        CERISE_AVANCE_DROIT,
+        CERISE_TOURNE_CONTACTEUR_GAUCHE,
+        CERISE_TOURNE_CONTACTEUR_DROIT,
+        CERISE_ACCOSTE
+    } etat_accostage=CERISE_AVANCE_DROIT;
+
+
+    // Position des contacteurs 
+    contacteur_gauche.x_mm = RAYON_ROBOT;
+    contacteur_gauche.y_mm = 0;
+
+    contacteur_droit.x_mm = RAYON_ROBOT * cos(-M_PI/6);
+    contacteur_droit.y_mm = RAYON_ROBOT * sin(-M_PI/6);
+
+    switch (etat_accostage)
+    {
+    case CERISE_AVANCE_DROIT:
+        commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
+        break;
+    
+    case CERISE_TOURNE_CONTACTEUR_GAUCHE:
+        rotation = ROTATION_CERISE;
+        commande_rotation(ROTATION_CERISE, contacteur_gauche.x_mm, contacteur_gauche.y_mm);
+        break;
+    
+    case CERISE_TOURNE_CONTACTEUR_DROIT:
+        rotation = ROTATION_CERISE;
+        commande_rotation(-ROTATION_CERISE, contacteur_droit.x_mm, contacteur_droit.y_mm);
+        break;
+    
+    default:
+        break;
+    }
+
+    return etat_action;
+}
+
diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h
new file mode 100644
index 0000000..e883c40
--- /dev/null
+++ b/Strategie_prise_cerises.h
@@ -0,0 +1 @@
+#include "Strategie.h"
\ No newline at end of file