author zer0 Fri, 29 Jan 2010 23:30:59 +0000 (00:30 +0100) committer zer0 Fri, 29 Jan 2010 23:30:59 +0000 (00:30 +0100)

index caa986c..f47068c 100644 (file)
@@ -475,6 +475,33 @@ void trajectory_manager_xy_event(struct trajectory *traj)
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 *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)
@@ -485,6 +512,8 @@ void trajectory_manager_circle_event(struct trajectory *traj)
int32_t d_consign = 0, a_consign = 0;
+       double coef_p, coef_d;
+       double d_speed, a_speed;

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

-       /* 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 > radius) {
-               angle_to_center_rad *= (M_PI / 2);
-       }
-       else {
-               angle_to_center_rad = 1. - (v2pol_target.r /
-       }

-       /* XXX check flags */
+       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 */
-       else {
-       }
+       angle_to_center_rad = v2pol_target.theta - (M_PI / 2.);
+       coef_d = coef_d;
+
+                                       &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",
+             coef_p, coef_d, d_speed, a_speed);

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

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

-       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",
+/*     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", */

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

-/*
- * 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 *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)
index bbcaacf..25a4745 100644 (file)
@@ -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
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
(parse_pgm_inst_t *)&cmd_test,

/* commands_traj.c */
index 77392ec..edf7601 100644 (file)
@@ -115,7 +115,7 @@ void mylog(struct error * e, ...)
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;
@@ -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));
-
+
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);
index cb620ca..140ef29 100644 (file)
@@ -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
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
(parse_pgm_inst_t *)&cmd_test,

/* commands_traj.c */
index 92f8969..f4fd855 100644 (file)
@@ -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_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 = {
index 3b2d82b..ca84489 100644 (file)
@@ -2197,6 +2197,44 @@ parse_pgm_inst_t cmd_beacon_opp_dump = {
};
#endif

+/**********************************************************/
+
+/* this structure is filled when cmd_circle_radius is parsed successfully */
+       fixed_string_t arg0;
+};
+                               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);
+       printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
+}
+
+
+       .f = cmd_circle_radius_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .tokens = {        /* token list, NULL terminated */
+               NULL,
+       },
+};
+
/**********************************************************/
/* Test */

index 9766ea9..678ebb8 100644 (file)
@@ -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_set_centrifugal_coef(&mainboard.pos, 0.000016);
+       position_set_centrifugal_coef(&mainboard.pos, 0.000016);
position_use_ext(&mainboard.pos);

/* TRAJECTORY MANAGER */
index d21cc9e..fac65cf 100644 (file)
@@ -33,7 +33,7 @@ def square(sz):
return sq

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

robot_trail = curve()
robot_trail_list = []
@@ -49,7 +49,7 @@ def set_robot(x, y, a):
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:
index 31a7832..d36fa5f 100644 (file)
@@ -94,7 +94,7 @@ int main(void)
\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