restore first circle algo + hostsim work

[aversive.git] / modules / devices / robot / trajectory_manager / trajectory_manager_core.c

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index a7473c4..caa986c 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -479,7 +479,7 @@ void trajectory_manager_xy_event(struct trajectory *traj)
 /* XXX static */
 void trajectory_manager_circle_event(struct trajectory *traj)
 {
-       double coef = 1.0;
+       double radius;
        double x = position_get_x_double(traj->position);
        double y = position_get_y_double(traj->position);
        double a = position_get_a_rad_double(traj->position);
@@ -504,11 +504,14 @@ 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;
+       radius = traj->target.circle.radius;
+       if (v2pol_target.r > radius) {
+               angle_to_center_rad = 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 * radius));
                angle_to_center_rad *= M_PI;
        }
 
@@ -520,8 +523,7 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        if (ABS(v2pol_target.theta) > traj->a_start_rad)
                set_quadramp_speed(traj, 0, traj->a_speed);
        else {
-               coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
-               set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
+               set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
        }
 
        /* XXX check flags */
@@ -546,9 +548,10 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 
        EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32
                  ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32
-                 ", a_speed=%" PRIi32,
-             rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
-             rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
+                 ", a_speed=%" PRIi32 "radius = %f",
+                 rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
+                 rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj),
+                 radius);
 
        cs_set_consign(traj->csm_angle, a_consign);
        cs_set_consign(traj->csm_distance, d_consign);