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=c6c70fcd38f76648b810fd44ee884d4ecb4a862f;hb=4abb0bf96078e303a8f43a0ba020a1801b908ae4;hpb=aed049ab6be4e1916457743d53f6b610a21b4854

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index c6c70fc..e3311a9 100644
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -186,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);
 }
@@ -196,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,
@@ -315,8 +318,12 @@ uint8_t trajectory_distance_finished(struct trajectory *traj)
  * distance. */
 uint8_t trajectory_finished(struct trajectory *traj)
 {
-	return trajectory_angle_finished(traj) &&
+	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 */
@@ -1004,15 +1011,15 @@ static void start_clitoid(struct trajectory *traj)
 	double R_mm = traj->target.line.R;
 	double d;
 
-	delete_event(traj);
 	DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
 	      __FUNCTION__, Va, Aa);
-	traj->state = RUNNING_CLITOID_CURVE;
+	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;
 }
 
 
@@ -1051,8 +1058,11 @@ int8_t trajectory_clitoid(struct trajectory *traj,
 	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)
+			 &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;