revert test serpi

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index f5e0add..85b2e57 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -504,17 +504,12 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        int32_t delta_d, delta_a;
        double coef_deriv = traj->circle_coef;
        double new_radius;
        int32_t delta_d, delta_a;
        double coef_deriv = traj->circle_coef;
        double new_radius;

-       double new_angle;

        struct  quadramp_filter * q_d, * q_a;
 
        q_d = traj->csm_distance->consign_filter_params;
        q_a = traj->csm_angle->consign_filter_params;
        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;
        delta_a = a_speed;//q_a->previous_var;
        delta_d = d_speed;//q_d->previous_var;

-       */
-       delta_a = q_a->previous_var;
-       delta_d = q_d->previous_var;

 
        /* step 1 : process new commands to quadramps */
 
 
        /* step 1 : process new commands to quadramps */
 


@@ -525,7 +520,6 @@ void trajectory_manager_circle_event(struct trajectory *traj)
        vect2_cart2pol(&v2cart_pos, &v2pol_target);
        v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 
        vect2_cart2pol(&v2cart_pos, &v2pol_target);
        v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 

-#if 0

        /* 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)
        /* 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)


@@ -575,53 +569,6 @@ void trajectory_manager_circle_event(struct trajectory *traj)
                              (traj->position->phys.track_mm) / 2.0);
        a_consign += rs_get_angle(traj->robot);
 
                              (traj->position->phys.track_mm) / 2.0);
        a_consign += rs_get_angle(traj->robot);
 

-#endif
-
-       d_speed/=100;
-       if (d_speed > v2pol_target.r)
-               d_speed = v2pol_target.r/2;
-       if (d_speed == 0)
-               d_speed = v2pol_target.r/2;
-       new_angle = (d_speed * d_speed);
-       new_angle += (v2pol_target.r * v2pol_target.r);
-       new_angle -= (traj->target.circle.radius * traj->target.circle.radius);
-       new_angle /= 2 * d_speed * v2pol_target.r;
-
-       printf("robot %f %f dspeed %d polr %f pola %f cnewa %f\n",
-              x, y,
-              d_speed,
-              v2pol_target.r, v2pol_target.theta * 180 / M_PI,
-              new_angle);
-
-       if (new_angle < -1)
-               new_angle = -1;
-
-       if (new_angle > 1)
-               new_angle = 1;
-
-
-       new_angle = acos(new_angle);
-       printf("new_a: %f\n", new_angle*180/M_PI);
-
-       new_angle = v2pol_target.theta - new_angle;
-       a_consign = (int32_t)(new_angle *
-                             (traj->position->phys.distance_imp_per_mm) *
-                             (traj->position->phys.track_mm) / 2.0);
-       a_consign += rs_get_angle(traj->robot);
-
-
-       /* XXX check flags */
-       d_consign = 40000 + rs_get_distance(traj->robot);
-
-
-
-
-
-
-
-
-
-

        /* step 2 : update state, or delete event if we reached the
         * destination */
 
        /* step 2 : update state, or delete event if we reached the
         * destination */
 

diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py
index 1e408c9..a6c90e7 100644 (file)
--- a/projects/microb2010/tests/hostsim/display.py
+++ b/projects/microb2010/tests/hostsim/display.py
@@ -48,7 +48,7 @@ while True:
             set_robot(x,y,a)
         f.close()
 
             set_robot(x,y,a)
         f.close()
 

-
+    """

     k = scene.kb.getkey()
     x,y,z = scene.center
     if k == "left":
     k = scene.kb.getkey()
     x,y,z = scene.center
     if k == "left":


@@ -59,5 +59,4 @@ while True:
         scene.center = x,y+10,z
     elif k == "down":
         scene.center = x,y-10,z
         scene.center = x,y+10,z
     elif k == "down":
         scene.center = x,y-10,z

-    elif k == "k":
-        robot_trail.pos = robot_trail.pos[0:0]
+    """

diff --git a/projects/microb2010/tests/hostsim/main.c b/projects/microb2010/tests/hostsim/main.c
index 3907c2a..0182943 100644 (file)
--- a/projects/microb2010/tests/hostsim/main.c
+++ b/projects/microb2010/tests/hostsim/main.c
@@ -72,19 +72,10 @@ int main(void)
        time_wait_ms(1000);\r
        printf("init\n");\r
        trajectory_d_rel(&mainboard.traj, 1000);\r
        time_wait_ms(1000);\r
        printf("init\n");\r
        trajectory_d_rel(&mainboard.traj, 1000);\r

-       time_wait_ms(1300);\r
-       /*\r
+       time_wait_ms(2000);\r

        printf("init\n");\r
        trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);\r
        time_wait_ms(2000);\r
        printf("init\n");\r
        trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);\r
        time_wait_ms(2000);\r

-       */\r
-       trajectory_circle_rel(&mainboard.traj,\r
-                             1000, 500,\r
-                             500,\r
-                             400,\r
-                             0);\r
-\r
-       time_wait_ms(10000);\r

        return 0;\r
 }\r
 \r
        return 0;\r
 }\r
 \r