other proto for circle

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index caa986c..f47068c 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -475,6 +475,33 @@ void trajectory_manager_xy_event(struct trajectory *traj)
        cs_set_consign(traj->csm_distance, d_consign);
 }
 
        cs_set_consign(traj->csm_distance, d_consign);
 }
 

+/*
+ * Compute the fastest distance and angle speeds matching the radius
+ * from current traj_speed
+ */
+/* static  */void circle_get_da_speed_from_radius(struct trajectory *traj,
+                                                 double radius_mm,
+                                                 double *speed_d,
+                                                 double *speed_a)
+{
+       /* speed_d = coef * speed_a */
+       double coef;
+       double speed_d2, speed_a2;
+
+       coef = 2. * radius_mm / traj->position->phys.track_mm;
+
+       speed_d2 = traj->a_speed * coef;
+       if (speed_d2 < traj->d_speed) {
+               *speed_d = speed_d2;
+               *speed_a = traj->a_speed;
+       }
+       else {
+               speed_a2 = traj->d_speed / coef;
+               *speed_d = traj->d_speed;
+               *speed_a = speed_a2;
+       }
+}
+

 /* trajectory event for circles */
 /* XXX static */
 void trajectory_manager_circle_event(struct trajectory *traj)
 /* trajectory event for circles */
 /* XXX static */
 void trajectory_manager_circle_event(struct trajectory *traj)


@@ -485,6 +512,8 @@ 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;
        double a = position_get_a_rad_double(traj->position);
        int32_t d_consign = 0, a_consign = 0;
        double angle_to_center_rad;

+       double coef_p, coef_d;
+       double d_speed, a_speed;

 
        /* These vectors contain target position of the robot in
         * its own coordinates */
 
        /* These vectors contain target position of the robot in
         * its own coordinates */


@@ -500,58 +529,57 @@ 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);
 

-       /* 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° */
+       /* radius consign */

        radius = traj->target.circle.radius;
        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 * radius));
-               angle_to_center_rad *= M_PI;
-       }

 
 

-       /* XXX check flags */
-       v2pol_target.theta -= angle_to_center_rad;
+       coef_p = v2pol_target.r / radius;
+       coef_p = 1. * coef_p;

 
 

-       /* If the robot is correctly oriented to start moving in distance */
-       /* here limit dist speed depending on v2pol_target.theta */
-       if (ABS(v2pol_target.theta) > traj->a_start_rad)
-               set_quadramp_speed(traj, 0, traj->a_speed);
-       else {
-               set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
-       }
+       angle_to_center_rad = v2pol_target.theta - (M_PI / 2.);
+       angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad);
+       if (angle_to_center_rad > 0.5)
+               angle_to_center_rad = 0.5;
+       if (angle_to_center_rad < -0.5)
+               angle_to_center_rad = -0.5;
+       coef_d = exp(5*angle_to_center_rad);
+       coef_d = coef_d;
+
+       circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d),
+                                       &d_speed, &a_speed);
+       set_quadramp_speed(traj, d_speed, a_speed);
+
+       EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f "
+             "d_speed=%2.2f a_speed=%2.2f",
+                 angle_to_center_rad, radius, v2pol_target.r,
+             coef_p, coef_d, d_speed, a_speed);

 
        /* XXX check flags */
 
        /* XXX check flags */

-       d_consign = 40000 + rs_get_distance(traj->robot);
+       d_consign = 400000 + rs_get_distance(traj->robot);
+       a_consign = 400000 + rs_get_angle(traj->robot);

 
        /* angle consign */
 
        /* angle consign */

-       a_consign = (int32_t)(v2pol_target.theta *
-                             (traj->position->phys.distance_imp_per_mm) *
-                             (traj->position->phys.track_mm) / 2.0);
-       a_consign += rs_get_angle(traj->robot);
+/*     a_consign = (int32_t)(v2pol_target.theta * */
+/*                           (traj->position->phys.distance_imp_per_mm) * */
+/*                           (traj->position->phys.track_mm) / 2.0); */
+/*     a_consign += rs_get_angle(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 */
 

-       /* output angle -> delete event */
-       if (a_consign >= traj->target.circle.dest_angle) {
-               a_consign = traj->target.circle.dest_angle;
-               delete_event(traj);
-       }
+/*     /\* output angle -> delete event *\/ */
+/*     if (a_consign >= traj->target.circle.dest_angle) { */
+/*             a_consign = traj->target.circle.dest_angle; */
+/*             delete_event(traj); */
+/*     } */

 
        /* step 3 : send the processed commands to cs */
 
 
        /* step 3 : send the processed commands to cs */
 

-       EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32
-                 ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32
-                 ", 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);
+/*     EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */
+/*               ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */
+/*               ", 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);
 
        cs_set_consign(traj->csm_angle, a_consign);
        cs_set_consign(traj->csm_distance, d_consign);


@@ -620,33 +648,6 @@ void trajectory_circle_rel(struct trajectory *traj,
        schedule_event(traj);
 }
 
        schedule_event(traj);
 }
 

-/*
- * Compute the fastest distance and angle speeds matching the radius
- * from current traj_speed
- */
-/* static  */void circle_get_da_speed_from_radius(struct trajectory *traj,
-                                                 double radius_mm,
-                                                 double *speed_d,
-                                                 double *speed_a)
-{
-       /* speed_d = coef * speed_a */
-       double coef;
-       double speed_d2, speed_a2;
-
-       coef = 2. * radius_mm / traj->position->phys.track_mm;
-
-       speed_d2 = traj->a_speed * coef;
-       if (speed_d2 < traj->d_speed) {
-               *speed_d = speed_d2;
-               *speed_a = traj->a_speed;
-       }
-       else {
-               speed_a2 = traj->d_speed / coef;
-               *speed_d = traj->d_speed;
-               *speed_a = speed_a2;
-       }
-}
-

 /* return the distance in millimeters that corresponds to an angle in
  * degree and a radius in mm */
 /* static  */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
 /* return the distance in millimeters that corresponds to an angle in
  * degree and a radius in mm */
 /* static  */double circle_get_dist_from_degrees(double radius_mm, double a_deg)

diff --git a/projects/microb2009/mainboard/commands.c b/projects/microb2009/mainboard/commands.c
index bbcaacf..25a4745 100644 (file)
--- a/projects/microb2009/mainboard/commands.c
+++ b/projects/microb2009/mainboard/commands.c
@@ -92,6 +92,7 @@ extern parse_pgm_inst_t cmd_build_z1;
 #ifdef TEST_BEACON
 extern parse_pgm_inst_t cmd_beacon_opp_dump;
 #endif
 #ifdef TEST_BEACON
 extern parse_pgm_inst_t cmd_beacon_opp_dump;
 #endif

+extern parse_pgm_inst_t cmd_circle_radius;

 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */


@@ -193,6 +194,7 @@ parse_pgm_ctx_t main_ctx[] = {
 #ifdef TEST_BEACON
        (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
 #endif
 #ifdef TEST_BEACON
        (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
 #endif

+       (parse_pgm_inst_t *)&cmd_circle_radius,

        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */
        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */

diff --git a/projects/microb2010/tests/hostsim/cmdline.c b/projects/microb2010/tests/hostsim/cmdline.c
index 77392ec..edf7601 100644 (file)
--- a/projects/microb2010/tests/hostsim/cmdline.c
+++ b/projects/microb2010/tests/hostsim/cmdline.c
@@ -115,7 +115,7 @@ void mylog(struct error * e, ...)
        if (e->severity > ERROR_SEVERITY_ERROR) {
                if (gen.log_level < e->severity)
                        return;
        if (e->severity > ERROR_SEVERITY_ERROR) {
                if (gen.log_level < e->severity)
                        return;

-               
+

                for (i=0; i<NB_LOGS+1; i++)
                        if (gen.logs[i] == e->err_num)
                                break;
                for (i=0; i<NB_LOGS+1; i++)
                        if (gen.logs[i] == e->err_num)
                                break;


@@ -126,12 +126,12 @@ void mylog(struct error * e, ...)
        va_start(ap, e);
        tv = time_get_time();
        printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL));
        va_start(ap, e);
        tv = time_get_time();
        printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL));

-       
+

        printf_P(PSTR("(%d,%d,%d) "),
                 position_get_x_s16(&mainboard.pos),
                 position_get_y_s16(&mainboard.pos),
                 position_get_a_deg_s16(&mainboard.pos));
        printf_P(PSTR("(%d,%d,%d) "),
                 position_get_x_s16(&mainboard.pos),
                 position_get_y_s16(&mainboard.pos),
                 position_get_a_deg_s16(&mainboard.pos));

-       
+

        vfprintf_P(stdout, e->text, ap);
        printf_P(PSTR("\r\n"));
        va_end(ap);
        vfprintf_P(stdout, e->text, ap);
        printf_P(PSTR("\r\n"));
        va_end(ap);

diff --git a/projects/microb2010/tests/hostsim/commands.c b/projects/microb2010/tests/hostsim/commands.c
index cb620ca..140ef29 100644 (file)
--- a/projects/microb2010/tests/hostsim/commands.c
+++ b/projects/microb2010/tests/hostsim/commands.c
@@ -92,6 +92,7 @@ extern parse_pgm_inst_t cmd_build_z1;
 #ifdef TEST_BEACON
 extern parse_pgm_inst_t cmd_beacon_opp_dump;
 #endif
 #ifdef TEST_BEACON
 extern parse_pgm_inst_t cmd_beacon_opp_dump;
 #endif

+extern parse_pgm_inst_t cmd_circle_radius;

 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */


@@ -193,6 +194,7 @@ parse_pgm_ctx_t main_ctx[] = {
 #ifdef TEST_BEACON
        (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
 #endif
 #ifdef TEST_BEACON
        (parse_pgm_inst_t *)&cmd_beacon_opp_dump,
 #endif

+       (parse_pgm_inst_t *)&cmd_circle_radius,

        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */
        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */

diff --git a/projects/microb2010/tests/hostsim/commands_gen.c b/projects/microb2010/tests/hostsim/commands_gen.c
index 92f8969..f4fd855 100644 (file)
--- a/projects/microb2010/tests/hostsim/commands_gen.c
+++ b/projects/microb2010/tests/hostsim/commands_gen.c
@@ -471,7 +471,7 @@ prog_char str_log_arg0[] = "log";
 parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
 prog_char str_log_arg1[] = "level";
 parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
 parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
 prog_char str_log_arg1[] = "level";
 parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);

-parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT8);

 
 prog_char help_log[] = "Set log options: level (0 -> 5)";
 parse_pgm_inst_t cmd_log = {
 
 prog_char help_log[] = "Set log options: level (0 -> 5)";
 parse_pgm_inst_t cmd_log = {

diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c
index 3b2d82b..ca84489 100644 (file)
--- a/projects/microb2010/tests/hostsim/commands_mainboard.c
+++ b/projects/microb2010/tests/hostsim/commands_mainboard.c
@@ -2197,6 +2197,44 @@ parse_pgm_inst_t cmd_beacon_opp_dump = {
 };
 #endif
 
 };
 #endif
 

+/**********************************************************/
+/* Circle_Radius */
+
+/* this structure is filled when cmd_circle_radius is parsed successfully */
+struct cmd_circle_radius_result {
+       fixed_string_t arg0;
+       int32_t radius;
+};
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+                               double radius_mm,
+                               double *speed_d,
+                               double *speed_a);
+/* function called when cmd_circle_radius is parsed successfully */
+static void cmd_circle_radius_parsed(void *parsed_result, void *data)
+{
+       struct cmd_circle_radius_result *res = parsed_result;
+       double d,a;
+       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+       circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
+       printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
+}
+
+prog_char str_circle_radius_arg0[] = "circle_radius";
+parse_pgm_token_string_t cmd_circle_radius_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_radius_result, arg0, str_circle_radius_arg0);
+parse_pgm_token_num_t cmd_circle_radius_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_circle_radius_result, radius, INT32);
+
+prog_char help_circle_radius[] = "Circle_Radius function";
+parse_pgm_inst_t cmd_circle_radius = {
+       .f = cmd_circle_radius_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_circle_radius,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_circle_radius_arg0,
+               (prog_void *)&cmd_circle_radius_arg1,
+               NULL,
+       },
+};
+

 /**********************************************************/
 /* Test */
 
 /**********************************************************/
 /* Test */
 

diff --git a/projects/microb2010/tests/hostsim/cs.c b/projects/microb2010/tests/hostsim/cs.c
index 9766ea9..678ebb8 100644 (file)
--- a/projects/microb2010/tests/hostsim/cs.c
+++ b/projects/microb2010/tests/hostsim/cs.c
@@ -168,7 +168,7 @@ void microb_cs_init(void)
        position_init(&mainboard.pos);
        position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
        position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
        position_init(&mainboard.pos);
        position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
        position_set_related_robot_system(&mainboard.pos, &mainboard.rs);

-       //position_set_centrifugal_coef(&mainboard.pos, 0.000016);
+       position_set_centrifugal_coef(&mainboard.pos, 0.000016);

        position_use_ext(&mainboard.pos);
 
        /* TRAJECTORY MANAGER */
        position_use_ext(&mainboard.pos);
 
        /* TRAJECTORY MANAGER */

diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py
index d21cc9e..fac65cf 100644 (file)
--- a/projects/microb2010/tests/hostsim/display.py
+++ b/projects/microb2010/tests/hostsim/display.py
@@ -33,7 +33,7 @@ def square(sz):
     return sq
 
 sq1 = square(250)
     return sq
 
 sq1 = square(250)

-sq1 = square(500)
+sq2 = square(500)

 
 robot_trail = curve()
 robot_trail_list = []
 
 robot_trail = curve()
 robot_trail_list = []


@@ -49,7 +49,7 @@ def set_robot(x, y, a):
                   0)
 
     # save position
                   0)
 
     # save position

-    save_pos.append(robot.pos.x, robot.pos, a)
+    save_pos.append((robot.pos.x, robot.pos, a))

 
     pos = robot.pos.x, robot.pos.y, 0.3
     if pos != last_pos:
 
     pos = robot.pos.x, robot.pos.y, 0.3
     if pos != last_pos:

diff --git a/projects/microb2010/tests/hostsim/main.c b/projects/microb2010/tests/hostsim/main.c
index 31a7832..d36fa5f 100644 (file)
--- a/projects/microb2010/tests/hostsim/main.c
+++ b/projects/microb2010/tests/hostsim/main.c
@@ -94,7 +94,7 @@ int main(void)
 \r
        mainboard.flags = DO_ENCODERS | DO_RS |\r
                DO_POS | DO_POWER | DO_BD | DO_CS;\r
 \r
        mainboard.flags = DO_ENCODERS | DO_RS |\r
                DO_POS | DO_POWER | DO_BD | DO_CS;\r

-       strat_reset_pos(1000, 1000, 0);\r
+       strat_reset_pos(1000, 1000, -90);\r

 \r
        cmdline_interact();\r
 \r
 \r
        cmdline_interact();\r
 \r