revert test serpi

[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 cc2a58a..85b2e57 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -485,12 +485,32 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        double a = position_get_a_rad_double(traj->position);
        int32_t d_consign = 0, a_consign = 0;
        double angle_to_center_rad;
+       static int32_t d_prev, a_prev;
+       int32_t d_speed, a_speed;
+       int32_t d_pos, a_pos;
+
+       d_pos = rs_get_distance(traj->robot);
+       a_pos = rs_get_angle(traj->robot);
+       d_speed = d_pos - d_prev;
+       a_speed = a_pos - a_prev;
+       d_prev = d_pos;
+       a_prev = a_pos;
 
        /* These vectors contain target position of the robot in
         * its own coordinates */
        vect2_cart v2cart_pos;
        vect2_pol v2pol_target;
 
+       int32_t delta_d, delta_a;
+       double coef_deriv = traj->circle_coef;
+       double new_radius;
+       struct  quadramp_filter * q_d, * q_a;
+
+       q_d = traj->csm_distance->consign_filter_params;
+       q_a = traj->csm_angle->consign_filter_params;
+       delta_a = a_speed;//q_a->previous_var;
+       delta_d = d_speed;//q_d->previous_var;
+
        /* step 1 : process new commands to quadramps */
 
        /* process the command vector from current position to the
@@ -500,17 +520,31 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        vect2_cart2pol(&v2cart_pos, &v2pol_target);
        v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 
+       /* pas trop mal, mais oscille */
+       //new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv;
+       if (v2pol_target.r > traj->target.circle.radius/2)
+               new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r;
+       else
+               new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ;
+
+       /* oscille a mort */
+       //new_radius = traj->target.circle.radius - traj->target.circle.radius   * delta_a * delta_a * coef_deriv;
+
+       /* ? */
+       //new_radius = traj->target.circle.radius - traj->target.circle.radius   * delta_a * coef_deriv;
+
+
        /* wanted direction of center of circle:
         * 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) {
-               angle_to_center_rad = traj->target.circle.radius / v2pol_target.r;
+       if (v2pol_target.r > new_radius) {
+               angle_to_center_rad = new_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 * new_radius));
                angle_to_center_rad *= M_PI;
        }
 
@@ -548,9 +582,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),
+                 new_radius);
 
        cs_set_consign(traj->csm_angle, a_consign);
        cs_set_consign(traj->csm_distance, d_consign);