From: zer0 <zer0@carbon.local>
Date: Sat, 9 Jan 2010 22:15:38 +0000 (+0100)
Subject: better orbiting
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=966d2c97e5fda37297568c5e8ed66271e986bcdd;p=aversive.git

better orbiting
---

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));