From 580718729e72aa18b9e247b8647b188ee195eabf Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 24 Apr 2010 23:01:24 +0200 Subject: [PATCH] debug in trajectory manager --- modules/debug/diagnostic/Makefile | 4 ++-- .../trajectory_manager/trajectory_manager_core.c | 13 ++++++++++--- .../trajectory_manager/trajectory_manager_core.h | 3 +++ 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/modules/debug/diagnostic/Makefile b/modules/debug/diagnostic/Makefile index 9f8da58..d4fdf06 100644 --- 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 18f5c90..e3afa27 100644 --- 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 + /** 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 b5a1395..87a955a 100644 --- 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. */ -- 2.39.5