From 966d2c97e5fda37297568c5e8ed66271e986bcdd Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 9 Jan 2010 23:15:38 +0100 Subject: [PATCH] better orbiting --- .../robot/trajectory_manager/trajectory_manager_core.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index a7473c4..cc2a58a 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -504,8 +504,10 @@ void trajectory_manager_circle_event(struct trajectory *traj) * if we are far, go in the center direction, * if we are at radius, we want to see the center at 90° * if we are nearer than radius, angle to center is > 90° */ - if (v2pol_target.r > traj->target.circle.radius * 2) - angle_to_center_rad = 0; + if (v2pol_target.r > traj->target.circle.radius) { + angle_to_center_rad = traj->target.circle.radius / v2pol_target.r; + angle_to_center_rad *= (M_PI / 2); + } else { angle_to_center_rad = 1. - (v2pol_target.r / (2 * traj->target.circle.radius)); -- 2.39.5