55 lines
1.4 KiB
C
55 lines
1.4 KiB
C
#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;
|
|
}
|
|
|