trajectory: follow a line

diff --git a/modules/base/math/geometry/lines.h b/modules/base/math/geometry/lines.h
index 5ae2394..6763b91 100755 (executable)
--- a/modules/base/math/geometry/lines.h
+++ b/modules/base/math/geometry/lines.h
@@ -29,11 +29,11 @@ typedef struct _line {
 } line_t;
 
 
 } line_t;
 
 

-void 
+void

 pts2line(const point_t *p1, const point_t *p2, line_t *l);
 
 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:
 
 /*
  * 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_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_ */
                  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 (file)
--- 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)
  *  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
  *  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 <aversive.h>
 #include <vect2.h>
 #include <robot_system.h>
 #include <aversive.h>
 #include <vect2.h>
 #include <robot_system.h>

+#include <vect_base.h>
+#include <lines.h>

 
 enum trajectory_state {
        READY,
 
 enum trajectory_state {
        READY,


@@ -44,7 +46,12 @@ enum trajectory_state {
        RUNNING_XY_B_START,
        RUNNING_XY_B_ANGLE,
        RUNNING_XY_B_ANGLE_OK,
        RUNNING_XY_B_START,
        RUNNING_XY_B_ANGLE,
        RUNNING_XY_B_ANGLE_OK,

+
+       /* circle */

        RUNNING_CIRCLE,
        RUNNING_CIRCLE,

+
+       /* line */
+       RUNNING_LINE,

 };
 
 struct circle_target {
 };
 
 struct circle_target {


@@ -57,6 +64,12 @@ struct circle_target {
        uint8_t flags;   /**< flags for this trajectory */
 };
 
        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 */
 
 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 */
                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) */
        } 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_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 */
                       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);
 
                                 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
  * 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.
  * 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)
  */
  * 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);
 
 /**
                            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);
 
 /** 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);
 
 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_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);
 
 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
 #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 (file)
--- 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 <aversive/error.h>
 #include <scheduler.h>
 #include <vect2.h>
 #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>
 
 #include <position_manager.h>
 #include <robot_system.h>


@@ -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)
                /* 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);
 }
 
        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 event */
 void trajectory_manager_event(void * param)
 {


@@ -607,6 +667,10 @@ void trajectory_manager_event(void * param)
                trajectory_manager_circle_event(traj);
                break;
 
                trajectory_manager_circle_event(traj);
                break;
 

+       case RUNNING_LINE:
+               trajectory_manager_line_event(traj);
+               break;
+

        default:
                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 (file)
--- 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;
 /* 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,
 };
 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)
 {
 /* 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);
 }
 
 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 = {
 
 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,
        .help_str = help_test,
        .tokens = {        /* token list, NULL terminated */
                (prog_void *)&cmd_test_arg0,

-               (prog_void *)&cmd_test_arg1,

                NULL,
        },
 };
                NULL,
        },
 };