63 lines
2.1 KiB
C
63 lines
2.1 KiB
C
|
#include "i2c_annexe.h"
|
||
|
|
#include "Localisation.h"
|
||
|
|
#include "Strategie.h"
|
||
|
|
#include "Trajectoire.h"
|
||
|
|
#include "Trajet.h"
|
||
|
|
#include "Geometrie.h"
|
||
|
|
|
||
|
|
enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
|
||
|
|
static enum {
|
||
|
|
GATEAU_PROCHE_PRE_POUSSE,
|
||
|
|
GATEAU_PROCHE_POUSSE,
|
||
|
|
GATEAU_PROCHE_RELACHE,
|
||
|
|
} etat_pousse_proche;
|
||
|
|
|
||
|
|
struct trajectoire_t trajectoire;
|
||
|
|
|
||
|
|
float angle_fin, point_x, point_y;
|
||
|
|
|
||
|
|
switch(etat_pousse_proche){
|
||
|
|
case GATEAU_PROCHE_PRE_POUSSE:
|
||
|
|
if(couleur == COULEUR_BLEU){
|
||
|
|
angle_fin = 90. * DEGRE_EN_RADIAN;
|
||
|
|
point_x = 310;
|
||
|
|
}else{
|
||
|
|
angle_fin = -135. * DEGRE_EN_RADIAN;
|
||
|
|
point_x = 1655;
|
||
|
|
}
|
||
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||
|
|
|
||
|
|
Trajet_config(250, 250);
|
||
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 360,
|
||
|
|
Localisation_get().angle_radian, angle_fin);
|
||
|
|
|
||
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
|
||
|
|
etat_pousse_proche = GATEAU_PROCHE_POUSSE;
|
||
|
|
i2c_annexe_deplie_bras();
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GATEAU_PROCHE_POUSSE:
|
||
|
|
if(couleur == COULEUR_BLEU){
|
||
|
|
angle_fin = 90. * DEGRE_EN_RADIAN;
|
||
|
|
point_x = 390;
|
||
|
|
}else{
|
||
|
|
angle_fin = -135. * DEGRE_EN_RADIAN;
|
||
|
|
point_x = 1585;
|
||
|
|
}
|
||
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||
|
|
|
||
|
|
Trajet_config(250, 250);
|
||
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1665,
|
||
|
|
Localisation_get().angle_radian, angle_fin);
|
||
|
|
|
||
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
|
||
|
|
etat_pousse_proche = GATEAU_PROCHE_POUSSE;
|
||
|
|
i2c_annexe_plie_bras();
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
|
||
|
|
case GATEAU_PROCHE_RELACHE:
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|