78 lines
2.3 KiB
C
78 lines
2.3 KiB
C
|
|
#include "Localisation.h"
|
|
#include "Strategie_prise_cerises.h"
|
|
#include "Strategie.h"
|
|
#include "Trajet.h"
|
|
#include "math.h"
|
|
|
|
#define DEGREE_EN_RADIAN (M_PI / 180.)
|
|
|
|
void Homologation(uint32_t step_ms){
|
|
static enum etat_strategie_t{
|
|
STRATEGIE_INIT,
|
|
APPROCHE_CERISE_1_A,
|
|
APPROCHE_CERISE_1_B,
|
|
ATTRAPE_CERISE_1,
|
|
APPROCHE_PANIER_1_A,
|
|
APPROCHE_PANIER_1_B,
|
|
CALAGE_PANIER_1,
|
|
STRATEGIE_FIN
|
|
}etat_strategie=STRATEGIE_INIT;
|
|
|
|
enum etat_action_t etat_action;
|
|
enum trajet_etat_t etat_trajet;
|
|
|
|
struct trajectoire_t trajectoire;
|
|
|
|
switch(etat_strategie){
|
|
case STRATEGIE_INIT:
|
|
Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
|
|
etat_strategie = APPROCHE_CERISE_1_A;
|
|
break;
|
|
|
|
case APPROCHE_CERISE_1_A:
|
|
Trajet_config(250, 500);
|
|
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN);
|
|
Trajet_debut_trajectoire(trajectoire);
|
|
etat_strategie = APPROCHE_CERISE_1_B;
|
|
break;
|
|
|
|
case APPROCHE_CERISE_1_B:
|
|
etat_trajet = Trajet_avance(step_ms/1000.);
|
|
if(etat_trajet == TRAJET_TERMINE){
|
|
etat_strategie = ATTRAPE_CERISE_1;
|
|
}
|
|
break;
|
|
|
|
case ATTRAPE_CERISE_1:
|
|
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
|
|
if(etat_action == ACTION_TERMINEE){
|
|
etat_strategie = APPROCHE_PANIER_1_A;
|
|
}
|
|
break;
|
|
|
|
case APPROCHE_PANIER_1_A:
|
|
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
|
485, Localisation_get().y_mm,
|
|
465, 857,
|
|
465,2830,
|
|
+30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
|
|
Trajet_debut_trajectoire(trajectoire);
|
|
etat_strategie = APPROCHE_PANIER_1_B;
|
|
break;
|
|
|
|
case APPROCHE_PANIER_1_B:
|
|
etat_trajet = Trajet_avance(step_ms/1000.);
|
|
if(etat_trajet == TRAJET_TERMINE){
|
|
etat_strategie = ATTRAPE_CERISE_1;
|
|
}
|
|
break;
|
|
|
|
|
|
|
|
case STRATEGIE_FIN:
|
|
break;
|
|
|
|
|
|
}
|
|
} |