From d1cc3debac68c0012f2c5ca33da9bcf0882f4988 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 3 Apr 2010 15:02:58 +0200 Subject: [PATCH] trajectory: follow a line --- modules/base/math/geometry/lines.h | 10 +- .../trajectory_manager/trajectory_manager.h | 42 +++++-- .../trajectory_manager_core.c | 103 +++++++++++++++++- .../tests/hostsim/commands_mainboard.c | 18 +-- 4 files changed, 138 insertions(+), 35 deletions(-) diff --git a/modules/base/math/geometry/lines.h b/modules/base/math/geometry/lines.h index 5ae2394..6763b91 100755 --- a/modules/base/math/geometry/lines.h +++ b/modules/base/math/geometry/lines.h @@ -29,11 +29,11 @@ typedef struct _line { } line_t; -void +void pts2line(const point_t *p1, const point_t *p2, line_t *l); void -proj_pt_line(const point_t * p, const line_t * l, point_t * p_out); +proj_pt_line(const point_t *p, const line_t *l, point_t *p_out); /* * return values: @@ -47,9 +47,9 @@ proj_pt_line(const point_t * p, const line_t * l, point_t * p_out); uint8_t intersect_line(const line_t *l1, const line_t *l2, point_t *p); -uint8_t -intersect_segment(const point_t *s1, const point_t *s2, - const point_t *t1, const point_t *t2, +uint8_t +intersect_segment(const point_t *s1, const point_t *s2, + const point_t *t1, const point_t *t2, point_t *p); #endif /* _LINES_H_ */ diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index f18e272..43730ea 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -25,6 +25,8 @@ #include #include #include +#include +#include enum trajectory_state { READY, @@ -44,7 +46,12 @@ enum trajectory_state { RUNNING_XY_B_START, RUNNING_XY_B_ANGLE, RUNNING_XY_B_ANGLE_OK, + + /* circle */ RUNNING_CIRCLE, + + /* line */ + RUNNING_LINE, }; struct circle_target { @@ -57,6 +64,12 @@ struct circle_target { uint8_t flags; /**< flags for this trajectory */ }; +struct line_target { + line_t line; + double angle; + double advance; +}; + struct trajectory { enum trajectory_state state; /*<< describe the type of target, and if we reached the target */ @@ -64,6 +77,7 @@ struct trajectory { vect2_cart cart; /**<< target, if it is a x,y vector */ struct rs_polar pol; /**<< target, if it is a d,a vector */ struct circle_target circle; /**<< target, if it is a circle */ + struct line_target line; /**<< target, if it is a line */ } target; double d_win; /**<< distance window (for END_NEAR) */ @@ -87,26 +101,26 @@ struct trajectory { void trajectory_init(struct trajectory *traj); /** structure initialization */ -void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, +void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, struct cs * cs_a); /** structure initialization */ -void trajectory_set_robot_params(struct trajectory *traj, - struct robot_system *rs, +void trajectory_set_robot_params(struct trajectory *traj, + struct robot_system *rs, struct robot_position *pos) ; /** set speed consign */ void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed); -/** - * set windows for trajectory. +/** + * set windows for trajectory. * params: distance window, angle window: we the robot enters this - * position window, we deletes the event and the last consign is + * position window, we deletes the event and the last consign is * used. - * a_start_deg used in xy consigns (start to move in distance when + * a_start_deg used in xy consigns (start to move in distance when * a_target < a_start) */ -void trajectory_set_windows(struct trajectory *traj, double d_win, +void trajectory_set_windows(struct trajectory *traj, double d_win, double a_win_deg, double a_start_deg); /** @@ -145,10 +159,10 @@ void trajectory_a_rel(struct trajectory *traj, double a_deg); /** go to angle 'a' in degrees */ void trajectory_a_abs(struct trajectory *traj, double a_deg); -/** turn the robot until the point x,y is in front of us */ +/** turn the robot until the point x,y is in front of us */ void trajectory_turnto_xy(struct trajectory*traj, double x_abs_mm, double y_abs_mm); -/** turn the robot until the point x,y is behind us */ +/** turn the robot until the point x,y is behind us */ void trajectory_turnto_xy_behind(struct trajectory*traj, double x_abs_mm, double y_abs_mm); /** update angle consign without changing distance consign */ @@ -183,4 +197,8 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r void trajectory_circle_rel(struct trajectory *traj, double x, double y, double radius_mm, double rel_a_deg, uint8_t flags); +/* do a line */ +void trajectory_line_abs(struct trajectory *traj, double x1, double y1, + double x2, double y2, double advance); + #endif //TRAJECTORY_MANAGER diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index f47068c..33c2a30 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include @@ -389,8 +391,6 @@ void trajectory_manager_xy_event(struct trajectory *traj) } } - /* XXX circle */ - /* 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) // || ABS(v2pol_target.r) < traj->d_win) @@ -585,6 +585,66 @@ void trajectory_manager_circle_event(struct trajectory *traj) cs_set_consign(traj->csm_distance, d_consign); } +/* trajectory event for lines */ +static void trajectory_manager_line_event(struct trajectory *traj) +{ + 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); + double advance, dist_to_line; + point_t robot, proj, target_pt; + int32_t d_consign = 0, a_consign = 0; + vect2_cart v2cart_pos; + vect2_pol v2pol_target; + + robot.x = x; + robot.y = y; + + /* target point on the line is further on the line */ + proj_pt_line(&robot, &traj->target.line.line, &proj); + dist_to_line = pt_norm(&robot, &proj); + if (dist_to_line > traj->target.line.advance) + advance = 0; + else + advance = traj->target.line.advance - dist_to_line; + target_pt.x = proj.x + advance * cos(traj->target.line.angle); + target_pt.y = proj.y + advance * sin(traj->target.line.angle); + + /* target vector */ + v2cart_pos.x = target_pt.x - x; + v2cart_pos.y = target_pt.y - y; + vect2_cart2pol(&v2cart_pos, &v2pol_target); + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); + + /* 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) // || ABS(v2pol_target.r) < traj->d_win) + set_quadramp_speed(traj, 0, traj->a_speed); + else { + double coef; + coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad; + set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed); + } + + /* position consign is infinite */ + d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + d_consign += rs_get_distance(traj->robot); + + /* angle consign */ + a_consign = (int32_t)(v2pol_target.theta * + (traj->position->phys.distance_imp_per_mm) * + (traj->position->phys.track_mm) / 2.2); + a_consign += rs_get_angle(traj->robot); + + EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f " + "a_consign=%"PRIi32" d_consign=%"PRIi32, + target_pt.x, target_pt.y, a_consign, d_consign); + + cs_set_consign(traj->csm_angle, a_consign); + cs_set_consign(traj->csm_distance, d_consign); +} + + /* trajectory event */ void trajectory_manager_event(void * param) { @@ -607,6 +667,10 @@ void trajectory_manager_event(void * param) trajectory_manager_circle_event(traj); break; + case RUNNING_LINE: + trajectory_manager_line_event(traj); + break; + default: break; } @@ -694,3 +758,38 @@ void trajectory_circle_abs_dist_deg(struct trajectory *traj, { } + +/*********** *LINE */ + +/* Follow a line */ +void trajectory_line_abs(struct trajectory *traj, + double x1, double y1, + double x2, double y2, + double advance) +{ + point_t p1, p2; + + delete_event(traj); + + /* find the line EQ */ + p1.x = x1; + p1.y = y1; + p2.x = x2; + p2.y = y2; + pts2line(&p1, &p2, &traj->target.line.line); + + /* find the line angle */ + traj->target.line.angle = atan2(y2-y1, x2-x1); + traj->target.line.advance = advance; + + DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f", + traj->target.line.line.a, + traj->target.line.line.b, + traj->target.line.line.c, + traj->target.line.angle); + + traj->state = RUNNING_LINE; + trajectory_manager_event(traj); + schedule_event(traj); +} + diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c index 782eac0..ff808c3 100644 --- a/projects/microb2010/tests/hostsim/commands_mainboard.c +++ b/projects/microb2010/tests/hostsim/commands_mainboard.c @@ -2241,7 +2241,6 @@ parse_pgm_inst_t cmd_circle_radius = { /* this structure is filled when cmd_test is parsed successfully */ struct cmd_test_result { fixed_string_t arg0; - int32_t radius; }; void circle_get_da_speed_from_radius(struct trajectory *traj, double radius_mm, @@ -2250,24 +2249,12 @@ void circle_get_da_speed_from_radius(struct trajectory *traj, /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - struct cmd_test_result *res = parsed_result; - double d,a; - uint8_t err; - - strat_reset_pos(1000, 500, 0); - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); - circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a); - trajectory_d_rel(&mainboard.traj, 1000); - err = WAIT_COND_OR_TRAJ_END(position_get_x_double(&mainboard.pos) > 1500, 0xFF); - if (err) - return; - strat_set_speed(d, a); - trajectory_d_a_rel(&mainboard.traj, 10000, 1000); + trajectory_line_abs(&mainboard.traj, 0, 1050, + 1500, 1050, 100.); } prog_char str_test_arg0[] = "test"; parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); -parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32); prog_char help_test[] = "Test function"; parse_pgm_inst_t cmd_test = { @@ -2276,7 +2263,6 @@ parse_pgm_inst_t cmd_test = { .help_str = help_test, .tokens = { /* token list, NULL terminated */ (prog_void *)&cmd_test_arg0, - (prog_void *)&cmd_test_arg1, NULL, }, }; -- 2.39.5