/** 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 !!! }