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