54 lines
1.5 KiB
C
54 lines
1.5 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;
|
|
|
|
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;
|
|
}
|
|
|
|
void commande_rotation_contacteur_longer_A(){
|
|
commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0);
|
|
}
|
|
|
|
void commande_rotation_contacteur_longer_C(){
|
|
commande_rotation(-ROTATION_CERISE, RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2);
|
|
} |