From fbac7e134e66ede2762e0c521f24d941cc413d13 Mon Sep 17 00:00:00 2001 From: Michal Vokac Date: Thu, 6 Jun 2013 20:01:45 +0200 Subject: [PATCH] robofsm: Arrive to target from angle between the target and playground center --- src/robofsm/common-states.cc | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index 30caa074..040cf628 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -49,6 +49,9 @@ std::vector detected_target; */ const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M; +const double xcenter = (PLAYGROUND_WIDTH_M / 2.0); +const double ycenter = ((PLAYGROUND_HEIGHT_M / 4.0) * 3.0); + void set_initial_position() { robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG)); @@ -137,15 +140,12 @@ static bool detect_target() */ void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach) { - double xrobot, yrobot, phi; double delta; - robot_get_est_pos(&xrobot, &yrobot, &phi); - - delta = distance(xrobot, yrobot, xtarget, ytarget); + delta = distance(xcenter, ycenter, xtarget, ytarget); - *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta); - *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta); + *xapproach = xtarget - (approach_radius * (xtarget - xcenter) / delta); + *yapproach = ytarget - (approach_radius * (ytarget - ycenter) / delta); *phi_approach = get_approach_angle(xtarget, ytarget); } @@ -158,11 +158,8 @@ void get_approach_point(double xtarget, double ytarget, double *xapproach, doubl */ double get_approach_angle(double xtarget, double ytarget) { - double xrobot, yrobot,phi; - - robot_get_est_pos(&xrobot, &yrobot, &phi); - return atan2((ytarget - yrobot), (xtarget - xrobot)); + return atan2((ytarget - ycenter), (xtarget - xcenter)); } -- 2.39.2