robofsm: Arrive to target from angle between the target and playground center

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 30caa0740fb897203197eea66d539d1b8a4bcfac..040cf62800a1a5bf596d2b5c2104974f16492152 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -49,6 +49,9 @@ std::vector<robot_pos_type> 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));
 }