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=e3311a9007b789cdf90c5b25f53b3b4486926c86;hp=33c2a3040a3838226d70161f10ae719ac69b576a;hb=4abb0bf96078e303a8f43a0ba020a1801b908ae4;hpb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index 33c2a30..e3311a9 100644
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -55,6 +55,7 @@ static uint8_t evt_debug_cpt = 0;
 		}					\
 	} while (0)
 
+static void start_clitoid(struct trajectory *traj);
 
 /**
  * update angle and/or distance
@@ -185,6 +186,7 @@ void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
 /** set relative angle and distance consign to 0 */
 void trajectory_stop(struct trajectory *traj)
 {
+	DEBUG(E_TRAJECTORY, "stop");
 	__trajectory_goto_d_a_rel(traj, 0, 0, READY,
 				  UPDATE_A | UPDATE_D | RESET_D | RESET_A);
 }
@@ -195,6 +197,8 @@ void trajectory_hardstop(struct trajectory *traj)
 {
 	struct quadramp_filter *q_d, *q_a;
 
+	DEBUG(E_TRAJECTORY, "hardstop");
+
 	q_d = traj->csm_distance->consign_filter_params;
 	q_a = traj->csm_angle->consign_filter_params;
 	__trajectory_goto_d_a_rel(traj, 0, 0, READY,
@@ -294,13 +298,32 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r
 
 /************ FUNCS FOR GETTING TRAJ STATE */
 
+uint8_t trajectory_angle_finished(struct trajectory *traj)
+{
+	return cs_get_consign(traj->csm_angle) ==
+		cs_get_filtered_consign(traj->csm_angle);
+}
+
+uint8_t trajectory_distance_finished(struct trajectory *traj)
+{
+	if (traj->state == RUNNING_CLITOID_CURVE)
+		return 1;
+
+	return cs_get_consign(traj->csm_distance) ==
+		cs_get_filtered_consign(traj->csm_distance) ;
+}
+
 /** return true if the position consign is equal to the filtered
  * position consign (after quadramp filter), for angle and
  * distance. */
 uint8_t trajectory_finished(struct trajectory *traj)
 {
-	return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
-		cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
+	uint8_t flags, ret;
+	IRQ_LOCK(flags);
+	ret = trajectory_angle_finished(traj) &&
+		trajectory_distance_finished(traj);
+	IRQ_UNLOCK(flags);
+	return ret;
 }
 
 /** return true if traj is nearly finished */
@@ -479,10 +502,10 @@ void trajectory_manager_xy_event(struct trajectory *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 radius_mm,
-						  double *speed_d,
-						  double *speed_a)
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+				     double radius_mm,
+				     double *speed_d,
+				     double *speed_a)
 {
 	/* speed_d = coef * speed_a */
 	double coef;
@@ -627,13 +650,11 @@ static void trajectory_manager_line_event(struct trajectory *traj)
 	}
 
 	/* position consign is infinite */
-	d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
+	d_consign = pos_mm2imp(traj, v2pol_target.r);
 	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);
+	/* angle consign (1.1 to avoid oscillations) */
+	a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1;
 	a_consign += rs_get_angle(traj->robot);
 
 	EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
@@ -642,6 +663,19 @@ static void trajectory_manager_line_event(struct trajectory *traj)
 
 	cs_set_consign(traj->csm_angle, a_consign);
 	cs_set_consign(traj->csm_distance, d_consign);
+
+	/* we reached dest, start clitoid */
+	if (traj->state == RUNNING_CLITOID_LINE &&
+	    xy_norm(proj.x,
+		    proj.y,
+		    traj->target.line.turn_pt.x,
+		    traj->target.line.turn_pt.y) <
+	    xy_norm(proj.x + cos(traj->target.line.angle),
+		    proj.y + sin(traj->target.line.angle),
+		    traj->target.line.turn_pt.x,
+		    traj->target.line.turn_pt.y)) {
+		start_clitoid(traj);
+	}
 }
 
 
@@ -668,6 +702,7 @@ void trajectory_manager_event(void * param)
 		break;
 
 	case RUNNING_LINE:
+	case RUNNING_CLITOID_LINE:
 		trajectory_manager_line_event(traj);
 		break;
 
@@ -762,15 +797,13 @@ 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)
+static 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;
@@ -788,8 +821,265 @@ void trajectory_line_abs(struct trajectory *traj,
 	      traj->target.line.line.c,
 	      traj->target.line.angle);
 
+}
+
+/* Follow a line */
+void trajectory_line_abs(struct trajectory *traj,
+			 double x1, double y1,
+			 double x2, double y2,
+			 double advance)
+{
+	delete_event(traj);
+	__trajectory_line_abs(traj, x1, y1, x2, y2, advance);
 	traj->state = RUNNING_LINE;
 	trajectory_manager_event(traj);
 	schedule_event(traj);
 }
 
+/*** CLOTHOID */
+
+/**
+ * process clitoid parameters
+ *
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ *            2 lines
+ *
+ * return 0 on success: in this case these parameters are filled:
+ * - Aa_out: the angular acceleration to configure in quadramp
+ * - Va_out: the angular speed to configure in quadramp
+ * - remain_d_mm_out: remaining distance before start to turn
+ */
+static int8_t calc_clitoid(struct trajectory *traj,
+			    double x, double y, double a_rad,
+			    double alpha_deg, double beta_deg, double R_mm,
+			    double Vd, double Amax, double d_inter_mm,
+			    double *Aa_out, double *Va_out, double *remain_d_mm_out)
+{
+	double Vd_mm_s;
+	double Va, Va_rd_s;
+	double t, tau, d_mm, alpha_rad, beta_rad;
+	double remain_d_mm;
+	double Aa, Aa_rd_s2;
+	line_t line1, line2;
+	line_t line1_int, line2_int;
+	point_t robot, intersect, pt2, center, proj, M;
+	vect_t v;
+	double xm, ym, L, A;
+
+	/* param check */
+	if (fabs(alpha_deg) <= fabs(beta_deg)) {
+		DEBUG(E_TRAJECTORY, "alpha is smaller than beta");
+		return -1;
+	}
+
+	/* get angular speed Va */
+	Vd_mm_s = speed_imp2mm(traj, Vd);
+	DEBUG(E_TRAJECTORY, "Vd_mm_s=%2.2f", Vd_mm_s);
+	Va_rd_s = Vd_mm_s / R_mm;
+	Va = speed_rd2imp(traj, Va_rd_s);
+	DEBUG(E_TRAJECTORY, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
+
+	/* process 't', the time in seconds that we will take to do
+	 * the first clothoid */
+	alpha_rad = RAD(alpha_deg);
+	beta_rad = RAD(beta_deg);
+	t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
+	DEBUG(E_TRAJECTORY, "R_mm=%2.2f a_rad=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
+	      R_mm, a_rad, alpha_rad, beta_rad, t);
+
+	/* process the angular acceleration */
+	Aa_rd_s2 = Va_rd_s / t;
+	Aa = acc_rd2imp(traj, Aa_rd_s2);
+	DEBUG(E_TRAJECTORY, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
+
+	/* exit if the robot cannot physically do it */
+	if (Aa > Amax) {
+		DEBUG(E_TRAJECTORY, "greater than max acceleration");
+		return -1;
+	}
+
+	/* define line1 and line2 */
+	robot.x = x;
+	robot.y = y;
+	intersect.x = x + cos(a_rad) * d_inter_mm;
+	intersect.y = y + sin(a_rad) * d_inter_mm;
+	pts2line(&robot, &intersect, &line1);
+	pt2.x = intersect.x + cos(a_rad + alpha_rad);
+	pt2.y = intersect.y + sin(a_rad + alpha_rad);
+	pts2line(&intersect, &pt2, &line2);
+	DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)",
+	      intersect.x, intersect.y);
+
+	/* L and A are the parameters of the clothoid, xm and ym are
+	 * the relative coords (starting from the beginning of
+	 * clothoid) of the crossing point between the clothoid and
+	 * the circle. */
+	L = Vd_mm_s * t;
+	A = R_mm * sqrt(fabs(alpha_rad - beta_rad));
+	xm =
+		L
+		- (pow(L, 5) / (40. * pow(A, 4)))
+		+ (pow(L, 9) / (3456. * pow(A, 8)))
+		- (pow(L, 13) / (599040. * pow(A, 12)));
+	ym =
+		(pow(L, 3) / (6. * pow(A, 2)))
+		- (pow(L, 7) / (336. * pow(A, 6)))
+		+ (pow(L, 11) / (42240. * pow(A, 10)))
+		- (pow(L, 15) / (9676800. * pow(A, 14)));
+	DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)",
+	      xm, ym);
+
+	/* the center of the circle is at d_mm when we have to start
+	 * the clothoid */
+	tau = (alpha_rad - beta_rad) / 2.;
+	d_mm = ym + (R_mm * cos(tau));
+	DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm);
+
+	/* translate line1 */
+	memcpy(&line1_int, &line1, sizeof(line1_int));
+	memcpy(&line2_int, &line2, sizeof(line2_int));
+	v.x = intersect.x - robot.x;
+	v.y = intersect.y - robot.y;
+	if (alpha_rad > 0)
+		vect_rot_trigo(&v);
+	else
+		vect_rot_retro(&v);
+	vect_resize(&v, d_mm);
+	line_translate(&line1_int, &v);
+	DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y);
+
+	/* translate line2_int */
+	v.x = intersect.x - pt2.x;
+	v.y = intersect.y - pt2.y;
+	if (alpha_rad < 0)
+		vect_rot_trigo(&v);
+	else
+		vect_rot_retro(&v);
+	vect_resize(&v, d_mm);
+	line_translate(&line2_int, &v);
+	DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y);
+
+	/* find the center of the circle, at the intersection of the
+	 * new translated lines */
+	if (intersect_line(&line1_int, &line2_int, &center) != 1) {
+		DEBUG(E_TRAJECTORY, "cannot find circle center");
+		return -1;
+	}
+	DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y);
+
+	/* M is the same point than xm, ym but in absolute coords */
+	if (alpha_rad < 0) {
+		M.x = center.x + cos(a_rad + M_PI/2 + tau) * R_mm;
+		M.y = center.y + sin(a_rad + M_PI/2 + tau) * R_mm;
+	}
+	else {
+		M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm;
+		M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm;
+	}
+	DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y);
+
+	/* project M on line 1 */
+	proj_pt_line(&M, &line1, &proj);
+	DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y);
+
+	/* process remaining distance before start turning */
+	remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm);
+	DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
+	if (remain_d_mm < 0) {
+		DEBUG(E_TRAJECTORY, "too late, cannot turn");
+		return -1;
+	}
+
+	/* return result */
+	*Aa_out = Aa;
+	*Va_out = Va;
+	*remain_d_mm_out = remain_d_mm;
+	return 0;
+}
+
+/* after the line, start the clothoid */
+static void start_clitoid(struct trajectory *traj)
+{
+	double Aa = traj->target.line.Aa;
+	double Va = traj->target.line.Va;
+	double a_rad = traj->target.line.alpha;
+	double R_mm = traj->target.line.R;
+	double d;
+
+	DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
+	      __FUNCTION__, Va, Aa);
+	delete_event(traj);
+	d = fabs(R_mm * a_rad);
+	d *= 3.; /* margin to avoid deceleration */
+	trajectory_d_a_rel(traj, d, DEG(a_rad));
+	set_quadramp_acc(traj, traj->d_acc, Aa);
+	set_quadramp_speed(traj, traj->d_speed, Va);
+	traj->state = RUNNING_CLITOID_CURVE;
+}
+
+
+/**
+ * do a superb curve joining line1 to line2 which is composed of:
+ *   - a clothoid starting from line1
+ *   - a circle
+ *   - another clothoid up to line2
+ * this curve is called a clitoid (hehe)
+ *
+ * the function assumes that the initial linear speed is Vd and
+ * angular speed is 0.
+ *
+ * - x,y,a_deg: starting position
+ * - advance: parameter for line following
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ *            2 lines
+ *
+ * return 0 if trajectory can be loaded, then it is processed in
+ * background.
+ */
+int8_t trajectory_clitoid(struct trajectory *traj,
+			  double x, double y, double a_deg, double advance,
+			  double alpha_deg, double beta_deg, double R_mm,
+			  double d_inter_mm)
+{
+	double remain = 0, Aa = 0, Va = 0, Vd;
+	double turnx, turny;
+	double a_rad = RAD(a_deg);
+
+	Vd = traj->d_speed;
+	if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm,
+			 Vd, traj->a_acc, d_inter_mm,
+			 &Aa, &Va, &remain) < 0) {
+		DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error",
+		      __FUNCTION__);
+		return -1;
+	}
+
+	delete_event(traj);
+	turnx = x + cos(a_rad) * remain;
+	turny = y + sin(a_rad) * remain;
+	traj->target.line.Aa = Aa;
+	traj->target.line.Va = Va;
+	traj->target.line.alpha = RAD(alpha_deg);
+	traj->target.line.R = R_mm;
+	traj->target.line.turn_pt.x = turnx;
+	traj->target.line.turn_pt.y = turny;
+	DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f",
+	      __FUNCTION__, turnx, turny);
+
+	__trajectory_line_abs(traj, x, y, turnx, turny,
+			      advance);
+	traj->state = RUNNING_CLITOID_LINE;
+	trajectory_manager_event(traj);
+	schedule_event(traj);
+	return 0;
+}