84 lines
2.7 KiB
C
84 lines
2.7 KiB
C
#include "i2c_annexe.h"
|
|
#include "Localisation.h"
|
|
#include "Monitoring.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 {
|
|
ALLER_1,
|
|
ALLER_2,
|
|
ALLER_3
|
|
} etat=ALLER_1;
|
|
float point_x, point_y;
|
|
float angle_fin;
|
|
struct trajectoire_t trajectoire;
|
|
|
|
|
|
switch(etat){
|
|
case ALLER_1:
|
|
if(couleur == COULEUR_BLEU){
|
|
angle_fin = 90. * DEGRE_EN_RADIAN;
|
|
point_x = 310;
|
|
}else{
|
|
angle_fin = 195. * DEGRE_EN_RADIAN;
|
|
point_x = 1700;
|
|
}
|
|
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
|
|
|
Trajet_config(100, 250);
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
|
etat=ALLER_2;
|
|
i2c_annexe_deplie_bras();
|
|
}
|
|
break;
|
|
|
|
case ALLER_2:
|
|
if(couleur == COULEUR_BLEU){
|
|
angle_fin = 180. * DEGRE_EN_RADIAN;
|
|
point_x = 225;
|
|
}else{
|
|
angle_fin = 180. * DEGRE_EN_RADIAN;
|
|
point_x = 1775;
|
|
}
|
|
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 - 1650,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
|
i2c_annexe_plie_bras();
|
|
etat=ALLER_3;
|
|
}
|
|
break;
|
|
|
|
case ALLER_3:
|
|
if(couleur == COULEUR_BLEU){
|
|
angle_fin = -150. * DEGRE_EN_RADIAN;
|
|
point_x = 225;
|
|
}else{
|
|
angle_fin = 30. * DEGRE_EN_RADIAN;
|
|
point_x = 1775;
|
|
}
|
|
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 - 1500,
|
|
Localisation_get().angle_radian, angle_fin);
|
|
|
|
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
|
etat=ALLER_1;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
} |