} 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:
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_ */
-/*
+/*
* 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
#include <aversive.h>
#include <vect2.h>
#include <robot_system.h>
+#include <vect_base.h>
+#include <lines.h>
enum trajectory_state {
READY,
RUNNING_XY_B_START,
RUNNING_XY_B_ANGLE,
RUNNING_XY_B_ANGLE_OK,
+
+ /* circle */
RUNNING_CIRCLE,
+
+ /* line */
+ RUNNING_LINE,
};
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 */
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) */
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);
/**
/** 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 */
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
#include <aversive/error.h>
#include <scheduler.h>
#include <vect2.h>
+#include <vect_base.h>
+#include <lines.h>
#include <position_manager.h>
#include <robot_system.h>
}
}
- /* 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)
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)
{
trajectory_manager_circle_event(traj);
break;
+ case RUNNING_LINE:
+ trajectory_manager_line_event(traj);
+ break;
+
default:
break;
}
{
}
+
+/*********** *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);
+}
+
/* 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,
/* 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 = {
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
(prog_void *)&cmd_test_arg0,
- (prog_void *)&cmd_test_arg1,
NULL,
},
};
git.droids-corp.org / aversive.git / commitdiff