void trajectory_goto_xy_pion_backward(trajectory_manager_t *t,trajectory_order_when_t when, double x, double y)
{
//y = y * get_couleur_depart();
//printf("GOTO xy %f %f %d\n",x,y,when);
trajectory_dest_t dest;
dest.type = XYPIONBACK;
dest.xy.x = position_cm2imp(t->pm,fxx_from_double(x));
dest.xy.y = position_cm2imp(t->pm,fxx_from_double(y));
dest.xy.prec_xy = position_cm2imp(t->pm,DEFAULT_PREC_DISTANCE);
dest.xy.prec_xy = dest.xy.prec_xy * dest.xy.prec_xy;
dest.xy.prec_ap = position_deg2imp(t->pm,DEFAULT_PREC_ANGLE_P);
//printf("DEBUG %f %f\n",x,y);
return trajectory_add_point(t,when,dest);
}
int8_t trajectory_goto_xya(trajectory_manager_t *t,trajectory_order_when_t when,double x,double y,double a)
{
//y = y * get_couleur_depart();
//a = a * get_couleur_depart();
trajectory_dest_t dest;
dest.type = XYA;
dest.xya.x = position_cm2imp(t->pm,fxx_from_double(x));
dest.xya.y = position_cm2imp(t->pm,fxx_from_double(y));
dest.xya.a = position_deg2imp(t->pm,fxx_from_double(a));
dest.xya.prec_xy = position_cm2imp(t->pm,DEFAULT_PREC_DISTANCE);
dest.xya.prec_xy = dest.xya.prec_xy * dest.xya.prec_xy;
dest.xya.prec_a = position_deg2imp(t->pm,DEFAULT_PREC_ANGLE);
dest.xya.prec_ap = position_deg2imp(t->pm,DEFAULT_PREC_ANGLE_P);
return trajectory_add_point(t,when,dest);
}
int8_t trajectory_goto_d(trajectory_manager_t *t,trajectory_order_when_t when,double d)
{
trajectory_dest_t dest;
dest.type = D;
dest.d.d = position_cm2imp(t->pm,fxx_from_double(d));
dest.d.prec_d = position_cm2imp(t->pm,DEFAULT_PREC_DISTANCE);
return trajectory_add_point(t,when,dest);
}
int8_t trajectory_goto_arel(trajectory_manager_t *t,trajectory_order_when_t when,double a)
{
while(a<-180)
{
a=a+360;
}
while(a>180)
{
a=a-360;
}
//a = a * get_couleur_depart();
trajectory_dest_t dest;
dest.type = AREL;
dest.a.a = position_deg2imp(t->pm,fxx_from_double(a));
dest.a.prec_a = position_deg2imp(t->pm,DEFAULT_PREC_ANGLE);
return trajectory_add_point(t,when,dest);
}
uint8_t findPosition(uint8_t type)
{
disableSpinning();
if(type == REDSTART)
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, -90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//asserv_set_distance()
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(20.0), fxx_from_double(0.0));
wait_ms(100);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
asserv_stop(&asserv);//attention bug bizare avant
asserv_set_vitesse_fast(&asserv);
}
else if(type == YELLOWSTART)// team == YELLOW
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, 90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(280.0), fxx_from_double(0.0));
asserv_stop(&asserv);
asserv_set_vitesse_fast(&asserv);
}
else if(type == REDPAINT)
{
// asserv_set_vitesse_low(&asserv);
// 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 * 1 + 7);
// 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 DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, 90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
//while(!trajectory_is_ended(&traj));
// if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
else if(type == YELLOWPAINT)
{
// 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 * 1 + 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 DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, -90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(-90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
//printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
quadramp_reset(&asserv);
control_reset(&asserv);
pid_reset(&asserv);
diff_reset(&asserv);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
sei();
enableSpinning();
return DONE;
}
void trajectory_tick(void *arg)
{
trajectory_manager_t * t = (trajectory_manager_t *)arg;
// printf("debugdispo %ld %ld\n",t->last,t->current);
if(t->current != t->last)
{
trajectory_dest_t *p = t->points + t->current;
if((!p->flags & ALREADY_INIT))
{
p->startingD = position_get_distance(t->pm);
p->startingA = position_get_angle(t->pm);
p->flags |= ALREADY_INIT;
if(p->type == XYPION || p->type == XYPIONBACK){
double acible = atan2(p->xy.y-position_get_y(t->pm),p->xy.x-position_get_x(t->pm));
p->xy.x-=position_cm2imp(t->pm,fxx_from_double(14))*cos(acible);
p->xy.y-=position_cm2imp(t->pm,fxx_from_double(14))*sin(acible);
}
}
if(p->type == D)
{
int32_t cons = p->d.d + p->startingD;
if(abs(cons - position_get_distance(t->pm)) < p->d.prec_d)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,p->startingA);
asserv_set_distance(t->ass,cons);
}
}
else if(p->type == D_NOA)
{
int32_t cons = p->d.d + p->startingD;
if(abs(cons - position_get_distance(t->pm)) < p->d.prec_d)
{
trajectory_next(t);
}
else
{
//asserv_set_no_angle(t->ass);
asserv_set_distance(t->ass,cons);
}
}
else if(p->type == AREL)
{
int32_t cons = p->a.a + p->startingA;
if(abs(cons - position_get_angle(t->pm)) < p->a.prec_a)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,cons);
}
}
else if(p->type == A)
{
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t cons = trajectory_get_nearest_angle(t,cur + p->a.a);
if(abs(cons - position_get_angle(t->pm)) < p->a.prec_a)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,cons);
}
}
else if(p->type == XYBACK || p->type == XYPIONBACK)
{
int32_t tarx = p->xy.x - position_get_x(t->pm);
int32_t tary = p->xy.y - position_get_y(t->pm);
if(tarx * tarx + tary * tary < (p->xya.prec_xy * p->xya.prec_xy)) //yavait pas le carré sur la précision avant !
{
trajectory_next(t);
}
else
{
int32_t acible = position_rad2imp(t->pm,fxx_from_double(atan2(tary,tarx)+M_PI));
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t acons = trajectory_get_nearest_angle(t,cur + acible);
asserv_set_angle(t->ass,acons);
if(abs(acons - position_get_angle(t->pm)) < p->xy.prec_ap)
{
int32_t dcons = -sqrt(tarx*tarx + tary*tary);
asserv_set_distance(t->ass,dcons + position_get_distance(t->pm));
}
else
{
asserv_set_distance(t->ass,position_get_distance(t->pm));
}
}
}
else if(p->type == XY || p->type == XYPION)
{
double tarx = p->xy.x - position_get_x(t->pm);
double tary = p->xy.y - position_get_y(t->pm);
if(tarx * tarx + tary * tary < p->xy.prec_xy) {
trajectory_next(t);
}
else {
int32_t acible = position_rad2imp(t->pm,fxx_from_double(atan2(tary,tarx)));
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t acons = trajectory_get_nearest_angle(t,cur + acible);
if(t->directed_flag) {
int32_t dcons = sqrt(tarx*tarx + tary*tary);
asserv_set_angle(t->ass,acons);
asserv_set_distance(t->ass, dcons + position_get_distance(t->pm));
}
else {
asserv_set_angle(t->ass,acons);
asserv_set_distance(t->ass, position_get_distance(t->pm));
if(abs(acons - position_get_angle(t->pm)) < p->xy.prec_ap)
t->directed_flag = 1;
}
}
}
else if(p->type == XYA)
{
double tarx = p->xya.x - position_get_x(t->pm);
double tary = p->xya.y - position_get_y(t->pm);
if(tarx * tarx + tary * tary < (p->xya.prec_xy * p->xya.prec_xy)) //cété pas au carré
{
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t cons = trajectory_get_nearest_angle(t,cur + p->xya.a);
if(abs(cons - position_get_angle(t->pm)) < p->xya.prec_a)
{
trajectory_next(t);
}
else
{
asserv_set_angle(t->ass,cons);
}
}
else
{
int32_t acible = position_rad2imp(t->pm,fxx_from_double(atan2(tary,tarx)));
int32_t cur = position_get_angle(t->pm) - position_get_angle_mod2pi(t->pm);
int32_t acons = trajectory_get_nearest_angle(t,cur + acible);
asserv_set_angle(t->ass,acons);
if(abs(acons - position_get_angle(t->pm)) < p->xya.prec_ap)
{
int32_t dcons = sqrt(tarx*tarx + tary*tary);
asserv_set_distance(t->ass,dcons + position_get_distance(t->pm));
}
else
{
asserv_set_distance(t->ass,position_get_distance(t->pm));
}
}
}
else if(p->type == WAIT)
{
// asserv_set_distance(t->ass,position_get_distance(t->pm));
// asserv_set_angle(t->ass,position_get_angle(t->pm));
asserv_set_distance(t->ass,p->startingD);
asserv_set_angle(t->ass,p->startingA);
//bloutre !
}
else if(p->type == STEP)
{
// asserv_set_distance(t->ass,position_get_distance(t->pm));
// asserv_set_angle(t->ass,position_get_angle(t->pm));
asserv_set_distance(t->ass,p->startingD);
asserv_set_angle(t->ass,p->startingA);
trajectory_next(t);
printf("END STEOP\n");
//notification_notify(t->not,NOTIFICATION_TRAJECTORY_END_STEP,NULL);
}
else //step
{
trajectory_next(t);
}
}
else
{
// asserv_set_distance(t->ass,position_get_distance(t->pm));
// asserv_set_angle(t->ass,position_get_angle(t->pm));
//notification_notify(t->not,NOTIFICATION_TRAJECTORY_END_FINAL,NULL);
}
}