/** near the target (dist in x,y) ? */
uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
{
double x1 = traj->target.cart.x;
double y1 = traj->target.cart.y;
double x2 = position_get_x_double(traj->position);
double y2 = position_get_y_double(traj->position);
return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
}
void rel_da_to_abs_xy(double d_rel, double a_rel_rad,
double *x_abs, double *y_abs)
{
double x = position_get_x_double(&mainboard.pos);
double y = position_get_y_double(&mainboard.pos);
double a = position_get_a_rad_double(&mainboard.pos);
*x_abs = x + d_rel*cos(a+a_rel_rad);
*y_abs = y + d_rel*sin(a+a_rel_rad);
}
/* return an angle between -pi and pi */
void abs_xy_to_rel_da(double x_abs, double y_abs,
double *d_rel, double *a_rel_rad)
{
double x = position_get_x_double(&mainboard.pos);
double y = position_get_y_double(&mainboard.pos);
double a = position_get_a_rad_double(&mainboard.pos);
*a_rel_rad = atan2(y_abs - y, x_abs - x) - a;
if (*a_rel_rad < -M_PI) {
*a_rel_rad += M_2PI;
}
else if (*a_rel_rad > M_PI) {
*a_rel_rad -= M_2PI;
}
*d_rel = norm(x_abs-x, y_abs-y);
}
uint8_t oa_do_i_need_to_stop(struct obstacle_avoidance * oa){
/*ir detected something... do we neeed to stop ???*/
struct vect2_cart cart_opp_rel, cart_opp_abs;
struct vect2_pol pol;
pol.r = 20; // if we've detected it, it's 20cm far only
pol.theta = position_get_a_rad_double(oa->traj->position);
//we get the cart from the pol and see if the cart is an element or outside of the table
vect2_pol2cart(&pol,&cart_opp_rel);
//now we got a relative position... let's check the absolute one..
cart_opp_abs.x = cart_opp_rel.x + position_get_x_double(oa->traj->position);
cart_opp_abs.y = cart_opp_rel.y + position_get_y_double(oa->traj->position);
//sprintf(uart_out_buffer,"oa> Opp @ x %d,y%d \r\n",(int16_t)cart_opp_abs.x,(int16_t)cart_opp_abs.y);
//UART_PutString(uart_out_buffer);
if(cart_opp_abs.x > 150 || cart_opp_abs.x < 50|| cart_opp_abs.y > 250 || cart_opp_abs.y < 50)
return 0;
else
return 1; //got to avoid it !!!
}