pathplan/robofsm: Repair constant for turning

[eurobot/public.git] / src / robofsm / motion-control.cc

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index c750bd19cc038756b5409abf5fb63fd774aa089a..0bd481a5cc1e051343b405fc0f620ea1fd4ee969 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -159,8 +159,8 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
                if (fabs(future_pos.v) < 0.01)
                        continue;
 
-               x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_BACK_M;
-               y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_BACK_M;
+               x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M;
+               y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M;
        
                ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
                if (!valid)
@@ -295,7 +295,7 @@ void *thread_trajectory_follower(void *arg)
                        next_period(&next, MOTION_PERIOD_NS);
                        if (measurement_ok) {
                                if (measurement_ok == 2) {
-                                       fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!");
+                                       fprintf(stderr, "problem: measurement timeout!\n");
                                }
                                measurement_ok--;
                        }