Samuel
3e5a8d0f4a
1 sursaut au rebroussement entre 1A et 1B et 1 sursaut au départ après le recalage
380 lines
12 KiB
C
380 lines
12 KiB
C
#include "Strategie.h"
|
|
#include "Asser_Position.h"
|
|
#include "Geometrie.h"
|
|
#include "Servomoteur.h"
|
|
|
|
|
|
|
|
enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){
|
|
static enum{
|
|
NINJA_INIT,
|
|
NINJA_GRENIER1,
|
|
NINJA_GRENIER2A,
|
|
NINJA_GRENIER2A_CALAGE,
|
|
NINJA_GRENIER2B,
|
|
NINJA_GRENIER2C,
|
|
NINJA_GRENIER2C_CALAGE,
|
|
NINJA_GRENIER2D,
|
|
NINJA_FIN
|
|
} etat_sss = NINJA_INIT;
|
|
static struct trajectoire_t trajectoire_composee;
|
|
static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
|
|
|
|
switch(etat_sss){
|
|
case NINJA_INIT:
|
|
if(couleur == COULEUR_JAUNE){
|
|
Localisation_set(645, 1870, 0);
|
|
Trajectoire_bezier(&trajectoire1,
|
|
645, 1870,
|
|
800, 1870,
|
|
890, 1730,
|
|
990, 1730, 0., 0.);
|
|
Trajectoire_droite(&trajectoire2,
|
|
990, 1730,
|
|
1150, 1730, 0, 0);
|
|
|
|
}else{
|
|
Localisation_set(3000 - 45, 1870, M_PI);
|
|
Trajectoire_bezier(&trajectoire1,
|
|
3000 - 45, 1870, 3000 - 800, 1870,
|
|
3000 - 890, 1730, 3000 - 990, 1730, 0., 0.);
|
|
Trajectoire_droite(&trajectoire2, 3000 - 990, 1730, 3000 - 1150, 1730, 0, 0);
|
|
}
|
|
|
|
Trajectoire_composee_init(&trajectoire_composee);
|
|
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
|
|
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
|
|
etat_sss = NINJA_GRENIER1;
|
|
break;
|
|
|
|
case NINJA_GRENIER1:
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2A;
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajectoire_bezier(&trajectoire1,
|
|
1150, 1730,
|
|
1030, 1730,
|
|
980, 1850,
|
|
980, 1970, 0., 0.);
|
|
}
|
|
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2A:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2A_CALAGE;
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2A_CALAGE:
|
|
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2B;
|
|
Localisation_set_y(1975);
|
|
Localisation_set_x(980);
|
|
Localisation_set_angle(-M_PI/2);
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajectoire_bezier(&trajectoire1,
|
|
980, 1975,
|
|
980, 1850,
|
|
1160, 1925,
|
|
1420, 1925, 0., 0.);
|
|
}
|
|
|
|
}
|
|
break;
|
|
|
|
|
|
case NINJA_GRENIER2B:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2C;
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajectoire_bezier(&trajectoire1,
|
|
1420, 1925,
|
|
1377, 1925,
|
|
1354, 1945,
|
|
1354, 1980, 0., 0.);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2C:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2C_CALAGE;
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2C_CALAGE:
|
|
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2D;
|
|
Localisation_set_y(1975);
|
|
Localisation_set_x(1354);
|
|
Localisation_set_angle(-M_PI/2);
|
|
Trajectoire_droite(&trajectoire1,
|
|
1354, 1975,
|
|
1354, 1630., 0., 0.);
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2D:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
case NINJA_FIN:
|
|
//Asser_Position_maintien();
|
|
return ACTION_TERMINEE;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
}
|
|
|
|
enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur){
|
|
static enum{
|
|
NINJA_INIT,
|
|
NINJA_GRENIER1A,
|
|
NINJA_GRENIER1B,
|
|
NINJA_GRENIER1B_RECALE,
|
|
NINJA_GRENIER1C,
|
|
NINJA_GRENIER1D,
|
|
NINJA_GRENIER2A,
|
|
NINJA_GRENIER2A_RECALE,
|
|
NINJA_GRENIER2B,
|
|
NINJA_GRENIER2C,
|
|
NINJA_FIN
|
|
} etat_sss = NINJA_INIT;
|
|
static struct trajectoire_t trajectoire_composee;
|
|
static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
|
|
|
|
switch(etat_sss){
|
|
case NINJA_INIT:
|
|
if(couleur == COULEUR_JAUNE){
|
|
Localisation_set(645, 1870, 0);
|
|
Trajectoire_bezier(&trajectoire1,
|
|
645, 1870,
|
|
935, 1870,
|
|
1025, 1920,
|
|
1315-50, 1920, 0., 0.);
|
|
|
|
}
|
|
etat_sss = NINJA_GRENIER1A;
|
|
break;
|
|
|
|
case NINJA_GRENIER1A:
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER1B;
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajet_config(100,100);
|
|
Trajectoire_bezier(&trajectoire1,
|
|
Localisation_get().x_mm, Localisation_get().y_mm,
|
|
1270-50, 1920,
|
|
1230-50, 1900,
|
|
1230-50, 1970, 0., 0.);
|
|
}
|
|
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER1B:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER1B_RECALE;
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER1B_RECALE:
|
|
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER1C;
|
|
Trajet_config(100,100);
|
|
Localisation_set(1230-50, 1975, -M_PI/2);
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajectoire_bezier(&trajectoire1,
|
|
1230-50, 1970,
|
|
1230, 1900,
|
|
1255, 1725,
|
|
1195, 1725, 0., 0.);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER1C:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER1D;
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
Trajectoire_droite(&trajectoire1,
|
|
1195, 1725,
|
|
1050, 1725, 0., 0.);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER1D:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2A;
|
|
Trajectoire_bezier(&trajectoire1,
|
|
1050, 1725,
|
|
1170, 1725,
|
|
1200, 1820,
|
|
1200, 1950,
|
|
0., 0.);
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2A:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2A_RECALE;
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2A_RECALE:
|
|
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2B;
|
|
Trajet_config(100, 100);
|
|
Localisation_set(1200, 1975, -M_PI/2);
|
|
if(couleur == COULEUR_JAUNE){
|
|
Trajectoire_bezier(&trajectoire1,
|
|
1200, 1975,
|
|
1200, 1890,
|
|
1135, 1775,
|
|
1260, 1775, 0., 0.);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2B:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_GRENIER2C;
|
|
Trajectoire_droite(&trajectoire1,
|
|
1260, 1775,
|
|
1400, 1775,
|
|
0., 0.);
|
|
}
|
|
break;
|
|
|
|
case NINJA_GRENIER2C:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_sss = NINJA_FIN;
|
|
Trajectoire_droite(&trajectoire1,
|
|
1260, 1775,
|
|
1400, 1775,
|
|
0., 0.);
|
|
}
|
|
break;
|
|
|
|
case NINJA_FIN:
|
|
//Asser_Position_maintien();
|
|
return ACTION_TERMINEE;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
|
|
}
|
|
|
|
enum etat_action_t Strategie_test(uint32_t step_ms){
|
|
static enum{
|
|
TEST_INIT,
|
|
TEST_EN_COURS,
|
|
TEST_TERMINE
|
|
} etat_test = TEST_INIT;
|
|
|
|
switch(etat_test){
|
|
case TEST_INIT:
|
|
if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){
|
|
etat_test = TEST_TERMINE;
|
|
}
|
|
break;
|
|
|
|
case TEST_TERMINE:
|
|
return ACTION_TERMINEE;
|
|
}
|
|
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
enum etat_action_t Strategie_1_metre(uint32_t step_ms){
|
|
static enum{
|
|
TEST_INIT,
|
|
TEST_EN_COURS,
|
|
TEST_TERMINE
|
|
} etat_test = TEST_INIT;
|
|
static struct trajectoire_t trajectoire1;
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
|
|
|
|
switch(etat_test){
|
|
case TEST_INIT:
|
|
Localisation_set(0, 0, 0);
|
|
Trajectoire_droite(&trajectoire1,
|
|
0, 0,
|
|
1000, 0, 0., 0.);
|
|
etat_test = TEST_EN_COURS;
|
|
break;
|
|
case TEST_EN_COURS:
|
|
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_test = TEST_TERMINE;
|
|
}
|
|
break;
|
|
|
|
case TEST_TERMINE:
|
|
return ACTION_TERMINEE;
|
|
}
|
|
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
|
|
void PAMI_dance(int identifiant){
|
|
switch (identifiant)
|
|
{
|
|
case 0:
|
|
sleep_ms(500);
|
|
Servomoteur_set(DOIGT_AVANCE);
|
|
sleep_ms(250);
|
|
Servomoteur_set(DOIGT_RETRAIT);
|
|
sleep_ms(250); // t = 1,0 s
|
|
sleep_ms(750); // t = 1,75 s
|
|
Servomoteur_set(DOIGT_AVANCE);
|
|
sleep_ms(250);
|
|
Servomoteur_set(DOIGT_RETRAIT);
|
|
sleep_ms(250); // t = 2,25 s
|
|
sleep_ms(950); // t = 3,2 s
|
|
for(int i =0; i<3;i++){
|
|
Servomoteur_set(DOIGT_AVANCE);
|
|
sleep_ms(200);
|
|
Servomoteur_set(DOIGT_RETRAIT);
|
|
sleep_ms(200);
|
|
}// t = 4,4 s
|
|
sleep_ms(1400); // t = 5,8
|
|
break;
|
|
case 1:
|
|
case 2:
|
|
case 3:
|
|
sleep_ms(1100); // t = 1,1 s
|
|
Servomoteur_set(DOIGT_AVANCE);
|
|
sleep_ms(250);
|
|
Servomoteur_set(DOIGT_RETRAIT);
|
|
sleep_ms(250); // t = 1,6 s
|
|
sleep_ms(800); // t = 2,4 s
|
|
Servomoteur_set(DOIGT_AVANCE);
|
|
sleep_ms(250);
|
|
Servomoteur_set(DOIGT_RETRAIT);
|
|
sleep_ms(250); // t = 2,9 s
|
|
sleep_ms(1600); // t = 4,5 s
|
|
for(int i =0; i<3;i++){
|
|
Servomoteur_set(DOIGT_AVANCE);
|
|
sleep_ms(150);
|
|
Servomoteur_set(DOIGT_RETRAIT);
|
|
sleep_ms(150);
|
|
}// t = 5,4 s
|
|
sleep_ms(400); // t = 5,8 s
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
} |