void print_positions(position_manager_t* pos)
{
printf("%f\t %f\t %f\t %ld\t %ld\t %d\t \n",
fxx_to_double(position_get_x_cm(pos)),
fxx_to_double(position_get_y_cm(pos)),
fxx_to_double(position_get_angle_deg(pos)),
POSX_FPGA,
POSY_FPGA,
ROT_FPGA);
}
void trajectory_goto_reculon_xy(trajectory_manager_t *t, trajectory_order_when_t chen, double x, double y)//Nouvelle fonction pour se déplacer plus simplement à reculon
{
//y = y * get_couleur_depart();
double x_current = position_get_x_cm(t->pm);
double y_current = position_get_y_cm(t->pm);//*get_couleur_depart();
//printf("%lf\t", x - x_current);
double angle = 180 + 180 * atan2(y - y_current, x - x_current) / M_PI;
//printf("%lf\n", angle);
double dist = -1 * sqrt((x - x_current)*(x - x_current) + (y - y_current)*(y - y_current));
trajectory_goto_a(t, END, angle); // relatif ou pas?
trajectory_goto_d(t, END, dist);
}
uint8_t putPaint(uint8_t side)
{
printf("putPaint \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
asserv_set_vitesse_normal(&asserv);
if(side == RED)
{
set_goalCoor(G_LENGTH * 3 + 7);
}
else
{
set_goalCoor(G_LENGTH * 3 + 8);
}
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
printf("angle %lf \n",position_get_angle_deg(&pos));
trajectory_goto_a(&traj, END, 180);
trajectory_goto_d(&traj, END, 10);
while(!trajectory_is_ended(&traj));
trajectory_goto_a(&traj, END, 0);
asserv_set_vitesse_normal(&asserv);
if(actionIsFailed())return FAILED;
disableAvoidance();
printf("angle %lf \n",position_get_angle_deg(&pos));
trajectory_goto_d(&traj, END, -50);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, 10);
while(!trajectory_is_ended(&traj));
// if(team == RED)
// {
// findPosition(REDPAINT);
// }
// else
// {
// findPosition(YELLOWPAINT);
// }
enableAvoidance();
return DONE;
}
uint8_t putFruit(uint8_t side)
{
asserv_set_vitesse_normal(&asserv);
printf("begin take fruit");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps);
static uint8_t k = 0;
if(team == RED)
{
set_goalCoor(G_LENGTH * 7 + 11);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps);
set_goalCoor(G_LENGTH * 3 + 11);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
actionSucceed();
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
trajectory_goto_a(&traj, END, 0);
while(!trajectory_is_ended(&traj));
if(actionIsFailed())return FAILED;
disableSpinning();
trajectory_goto_d(&traj, END, -40);
enableSpinning();
while(!trajectory_is_ended(&traj));
// {
// if(REPOSITIONING)
// {
// trajectory_reinit(&traj);
// asserv_stop(&asserv);
// }
// }
enableSpinning();
//se recaller?
throwSpears(YELLOW);
trajectory_goto_d(&traj, END, -4);
while(!trajectory_is_ended(&traj));
if(k)
{
trajectory_goto_a(&traj, END, -90);
}
else
{
trajectory_goto_a(&traj, END, -90);
}
disableAvoidance();
//trajectory_goto_d(&traj, END, -40);
while(!trajectory_is_ended(&traj));
enableAvoidance();
//if(actionIsFailed())return FAILED;
mecaCom(TIROIR_DEVERSER);
wait_ms(2000);
mecaCom(TIROIR_FERMER);
//trajectory_goto_d(&traj, END, -60);
//trajectory_goto_a(&traj, END, 0);
//trajectory_goto_d(&traj, END,15);
//while(!trajectory_is_ended(&traj));
}
else
{
set_goalCoor(G_LENGTH * 7 + 3);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps);
set_goalCoor(G_LENGTH * 4 + 3);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
actionSucceed();
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
trajectory_goto_a(&traj, END, 180);
trajectory_goto_d(&traj, END, 10);
trajectory_goto_a(&traj, END, 0);
while(!trajectory_is_ended(&traj));
if(actionIsFailed())return FAILED;
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj));
// {
// if(REPOSITIONING)
// {
// trajectory_reinit(&traj);
// asserv_stop(&asserv);
// }
// }
enableSpinning();
throwSpears(RED);
trajectory_goto_d(&traj, END, -5);
while(!trajectory_is_ended(&traj));
if(k)
{
trajectory_goto_a(&traj, END, -90);
}
else
{
trajectory_goto_a(&traj, END, -90);
}
disableAvoidance();
while(!trajectory_is_ended(&traj));
enableAvoidance();
// if(actionIsFailed())return FAILED;
mecaCom(TIROIR_DEVERSER);
wait_ms(2000);
mecaCom(TIROIR_FERMER);
// if(k)
// {
// trajectory_goto_a(&traj, END, 0);
// trajectory_goto_d(&traj, END, 12);
// }
// else
// {
// trajectory_goto_a(&traj, END, 90);
// k = 1;
// //trajectory_goto_d(&traj, END, 0);
// }
// while(!trajectory_is_ended(&traj));
}
return DONE;
}
uint8_t takeFruitYellow(uint8_t type)
{
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
set_goalCoor(G_LENGTH * 8 + 9);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
printf("take the fruit");
mecaCom(PEIGNE_OUVERT);
mecaCom(TIROIR_OUVERT);
trajectory_goto_a(&traj, END, 0);
if (team == RED)
{
trajectory_goto_d(&traj, END, 4);
}
else
{
trajectory_goto_d(&traj, END, 10);
}
trajectory_goto_a(&traj, END, 90);
while(!trajectory_is_ended(&traj));
if(actionIsFailed())return FAILED;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_d(&traj, END, 80);
trajectory_goto_a(&traj, END, 135);
if(team == RED)
{
trajectory_goto_d(&traj, END, 11 * 1.414);
}
else
{
trajectory_goto_d(&traj, END, 11 * 1.414);
}
// trajectory_goto_a(&traj, END, 90);
// while(!trajectory_is_ended(&traj));
// trajectory_goto_d(&traj, END, 12);
// while(!trajectory_is_ended(&traj));
trajectory_goto_a(&traj, END, 180);
trajectory_goto_d(&traj, END, 55);
trajectory_goto_a(&traj, END, -90);
if(team == RED)
{
trajectory_goto_d(&traj, END, 11 * 1.414);
}
else
{
trajectory_goto_d(&traj, END, 12 * 1.414);
}
while(!trajectory_is_ended(&traj));
mecaCom(PEIGNE_FERMER);
asserv_set_vitesse_normal(&asserv);
if(actionIsFailed())return FAILED;
return DONE;
}
void test_evitement(void)
{
for(int i = 0;i < 32;i++)
{
trajectory_goto_arel(&traj, END, 45);
while(!trajectory_is_ended(&traj));
}
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
// addTask(&tkm, &throwSpears, LOW_PRIORITY, YELLOW);
// addTask(&tkm, &returnFire, LOW_PRIORITY, Y2);
// // addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, YELLOWPAINT);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R1);
// addTask(&tkm, &throwSpears, HIGH_PRIORITY, RED);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
// asserv_set_vitesse_normal(&asserv);
// while (TIRETTE);
// printf("begin match \n");
// avoidance_init();
// while(doNextTask(&tkm));
// printf("finish \n"); // throwSpears(RED);
// printf("succeed");
// trajectory_goto_d(&traj, END, 10);
// while(!trajectory_is_ended(&traj));
// printf("astar test \n");
// set_startCoor(G_LENGTH * 2 + 2);
// set_goalCoor(G_LENGTH * 8 + 13);
// while(!astarMv() && !actionIsFailed());
// trajectory_goto_d(&traj, END, -100);
// while(!trajectory_is_ended(&traj));
while(1);
// trajectory_goto_arel(&traj, END, 180);
// while(!trajectory_is_ended(&traj));
// trajectory_goto_d(&traj, END, 100);
// while(!trajectory_is_ended(&traj));
//
double x = fxx_to_double(position_get_y_cm(&pos));
double y = fxx_to_double(position_get_x_cm(&pos));
printf("x : %lf y : %lf\n", x, y);
printf("isOut %d obs %d\n", isOutOfGraphe(x, y), isObstacle(x, y));
//go_to_node(x, y,graphe);
}
uint8_t returnFire(uint8_t fireNb)
{
static uint8_t tab[7] = {0};
for(uint8_t i = 1;i < 7;i++)
{
if(!(tab[i] & DONE))
{
avoidFire(i);
}
}
uint16_t stackend = 0;
printf("stack %d \n",&stackend - &stack_begin);
if(fireNb == R1)
{
printf("return R1 \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 3 + 3);
}
else
{
set_goalCoor(G_LENGTH * 3 + 6);
asserv_set_vitesse_normal(&asserv);
}
//put a wall to avoid the wrong way
avoidFire(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
//test if it succeed
eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 3 + 5);
}
else
{
set_goalCoor(G_LENGTH * 3 + 4);
}
//remove the wall
stopAvoid(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
}
else if(fireNb == R2)
{
printf("return R2 \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 4 + 2);
}
else
{
set_goalCoor(G_LENGTH * 7 + 2);
}
//put a wall to avoid the wrong way
avoidFire(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
//test if it succeed
eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 6 + 2);
}
else
{
set_goalCoor(G_LENGTH * 5 + 2);
}
//remove the wall
stopAvoid(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
}
else if(fireNb == R3)
{
printf("return R3 \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 8 + 6);
}
else
{
set_goalCoor(G_LENGTH * 8 + 3);
}
//put a wall to avoid the wrong way
avoidFire(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
//test if it succeed
eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 8 + 4);
}
else
{
set_goalCoor(G_LENGTH * 8 + 5);
}
//remove the wall
stopAvoid(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
}
else if(fireNb == Y1)
{
printf("return Y1 \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 3 + 9);
}
else
{
set_goalCoor(G_LENGTH * 3 + 12);
}
//put a wall to avoid the wrong way
avoidFire(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
//test if it succeed
eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 3 + 11);
}
else
{
set_goalCoor(G_LENGTH * 3 + 10);
}
//remove the wall
stopAvoid(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
}
else if(fireNb == Y2)
{
printf("return Y2 \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 7 + 13);
}
else
{
set_goalCoor(G_LENGTH * 4 + 13);
}
//put a wall to avoid the wrong way
avoidFire(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
//test if it succeed
eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 5 + 13);
}
else
{
set_goalCoor(G_LENGTH * 6 + 13);
}
//remove the wall
stopAvoid(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
}
else if(fireNb == Y3)
{
printf("return Y3 \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 8 + 12);
}
else
{
set_goalCoor(G_LENGTH * 8 + 9);
}
//put a wall to avoid the wrong way
avoidFire(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
//test if it succeed
eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
if(team == RED)
{
set_goalCoor(G_LENGTH * 8 + 10);
}
else
{
set_goalCoor(G_LENGTH * 8 + 11);
}
//remove the wall
stopAvoid(fireNb);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
}
else
{
}
//if succeed
tab[fireNb] += DONE;
for(uint8_t i = 1;i < 7;i++)
{
if(!(tab[i] & DONE))
{
stopAvoid(i);
}
}
return DONE;
}
uint8_t throwSpears(uint8_t side)
{
if(side == RED)
{
if(team == RED)
{
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
set_goalCoor(G_LENGTH * 3 + 3);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv());
trajectory_goto_a(&traj, END, 90);
trajectory_goto_d(&traj, END, 11);
while(!trajectory_is_ended(&traj));
disableAvoidance();//test
trajectory_goto_a(&traj, END, -180);
trajectory_goto_d(&traj, END, 14.8);
while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AV);
trajectory_goto_d(&traj, END, -14.8);
while(!trajectory_is_ended(&traj));
enableAvoidance();//test
}
else
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos,&eps));
// set_goalCoor(G_LENGTH * 3 + 4);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// trajectory_goto_a(&traj,END,0);
// trajectory_goto_d(&traj, END, -16.8);
// while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AR);
// trajectory_goto_d(&traj, END, 16.8);
// while(!trajectory_is_ended(&traj));
}
}
else
{
if(team == RED)
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos,&eps));
// set_goalCoor(G_LENGTH * 3 + 11);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv());
// trajectory_goto_a(&traj, END, 0);
// trajectory_goto_d(&traj, END, -16.8);
// while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AR);
// trajectory_goto_d(&traj, END, 16.8);
// while(!trajectory_is_ended(&traj));
}
else
{
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
set_goalCoor(G_LENGTH * 3 + 12);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
trajectory_goto_a(&traj,END,-90);
trajectory_goto_d(&traj, END, 11);
while(!trajectory_is_ended(&traj));
disableAvoidance();//test
trajectory_goto_a(&traj,END,180);
trajectory_goto_d(&traj, END, 14.8);
while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AV);
trajectory_goto_d(&traj, END, -14.8);
while(!trajectory_is_ended(&traj));
enableAvoidance();//test
}
}
return DONE;
}
