debug in trajectory manager

diff --git a/modules/debug/diagnostic/Makefile b/modules/debug/diagnostic/Makefile
index 9f8da585798f1204aa4234bce0449815b376ab21..d4fdf06573b06bfe0e98855a515ee6dac30071e4 100644 (file)
--- a/modules/debug/diagnostic/Makefile
+++ b/modules/debug/diagnostic/Makefile
@@ -2,9 +2,9 @@ TARGET = diagnostic
 
 # List C source files here. (C dependencies are automatically generated.)
 ifeq ($(HOST),avr)
-SRC = stack_space.c int_show.c
+SRC = stack_space.c int_show.c stackdump.c
 else
-SRC = diag_host.c
+SRC = diag_host.c stackdump.c
 endif
 
 include $(AVERSIVE_DIR)/mk/aversive_module.mk

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index 18f5c9031f5648f9e0760211851d745a3f401408..e3afa27c91c45469530a08a2a5f16bee9f09b146 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -186,16 +186,21 @@ 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);
 }
 
+#include <stackdump.h>
+
 /** set relative angle and distance consign to 0, and break any
  * deceleration ramp in quadramp filter */
 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,
@@ -1004,15 +1009,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);
+       delete_event(traj);
        d = fabs(R_mm * a_rad);
        d *= 3.; /* margin to avoid deceleration */
        trajectory_d_a_rel(traj, d, DEG(a_rad));
-       traj->state = RUNNING_CLITOID_CURVE;
        set_quadramp_acc(traj, traj->d_acc, Aa);
        set_quadramp_speed(traj, traj->d_speed, Va);
+       traj->state = RUNNING_CLITOID_CURVE;
 }
 
 
@@ -1051,8 +1056,10 @@ 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");
                return -1;
+       }
 
        delete_event(traj);
        turnx = x + cos(a_rad) * remain;

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h
index b5a13957e53659c8d20042f2e560837237ff0726..87a955a3367bb252ab1bc6af088af83b155ab5e2 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h
@@ -89,6 +89,9 @@ 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);
+uint8_t trajectory_distance_finished(struct trajectory *traj);
+
 /** return true if the position consign is equal to the filtered
  * position consign (after quadramp filter), for angle and
  * distance. */