Début de l'algo pour attraper les cerises - ajout de la fonction de rotation autour d'un point.

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

Commande_vitesse.h

@@ -1 +1,2 @@
-void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s);
+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);

Geometrie_robot.h

@@ -1,2 +1,3 @@
 #define DISTANCE_ROUES_CENTRE_MM 84.25
+#define RAYON_ROBOT 125
 #define RACINE_DE_3 1.73205081

Strategie.h

@@ -0,0 +1,4 @@
+enum etat_action_t{
+    ACTION_EN_COURS,
+    ACTION_TERMINEE
+};

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;
+}
+

Strategie_prise_cerises.h

@@ -0,0 +1 @@
+#include "Strategie.h"