X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager_core.c;h=33c2a3040a3838226d70161f10ae719ac69b576a;hp=f47068cc6f6a75ffca427fbb0e8d945a40544104;hb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988;hpb=09e0cfb842943982e9fa3c4792c097bf4be25360;ds=sidebyside

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 <aversive/error.h>
 #include <scheduler.h>
 #include <vect2.h>
+#include <vect_base.h>
+#include <lines.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)
@@ -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);
+}
+