87 lines
2.8 KiB
C
87 lines
2.8 KiB
C
#include "Strategie_prise_cerises.h"
|
|
#include "Commande_vitesse.h"
|
|
#include "Geometrie.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "math.h"
|
|
#include "i2c_annexe.h"
|
|
|
|
// Rotation en rad/s pour accoster les cerises
|
|
#define ROTATION_CERISE 0.5f
|
|
|
|
void commande_rotation_contacteur_longer_A();
|
|
void commande_rotation_contacteur_longer_C();
|
|
|
|
|
|
double vitesse_accostage_mm_s=100;
|
|
|
|
enum etat_action_t cerise_accostage(void){
|
|
enum etat_action_t etat_action = ACTION_EN_COURS;
|
|
double rotation;
|
|
|
|
static enum {
|
|
CERISE_AVANCE_DROIT,
|
|
CERISE_TOURNE_CONTACTEUR_LONGER_A,
|
|
CERISE_TOURNE_CONTACTEUR_LONGER_C,
|
|
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);
|
|
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
|
|
etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A;
|
|
}
|
|
if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
|
|
etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C;
|
|
}
|
|
if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
|
|
etat_accostage=CERISE_ACCOSTE;
|
|
}
|
|
break;
|
|
|
|
case CERISE_TOURNE_CONTACTEUR_LONGER_A:
|
|
commande_rotation_contacteur_longer_A();
|
|
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
|
|
if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
|
|
etat_accostage = CERISE_AVANCE_DROIT;
|
|
}else{
|
|
etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A;
|
|
}
|
|
}else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
|
|
etat_accostage = CERISE_ACCOSTE;
|
|
}
|
|
break;
|
|
|
|
case CERISE_TOURNE_CONTACTEUR_LONGER_C:
|
|
commande_rotation_contacteur_longer_C();
|
|
if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
|
|
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
|
|
etat_accostage = CERISE_AVANCE_DROIT;
|
|
}else{
|
|
etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C;
|
|
}
|
|
}else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
|
|
etat_accostage = CERISE_ACCOSTE;
|
|
}
|
|
break;
|
|
|
|
case CERISE_ACCOSTE:
|
|
commande_vitesse_stop();
|
|
//etat_accostage = CERISE_AVANCE_DROIT;
|
|
etat_action = ACTION_TERMINEE;
|
|
|
|
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);
|
|
} |