git.droids-corp.org
/
aversive.git
/ commitdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
| commitdiff |
tree
raw
|
patch
|
inline
| side by side (from parent 1:
e442b9c
)
debug in trajectory manager
author
zer0
<zer0@carbon.local>
Sat, 24 Apr 2010 21:01:24 +0000
(23:01 +0200)
committer
zer0
<zer0@carbon.local>
Sat, 24 Apr 2010 21:01:24 +0000
(23:01 +0200)
modules/debug/diagnostic/Makefile
patch
|
blob
|
history
modules/devices/robot/trajectory_manager/trajectory_manager_core.c
patch
|
blob
|
history
modules/devices/robot/trajectory_manager/trajectory_manager_core.h
patch
|
blob
|
history
diff --git
a/modules/debug/diagnostic/Makefile
b/modules/debug/diagnostic/Makefile
index
9f8da58
..
d4fdf06
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)
# 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
else
-SRC = diag_host.c
+SRC = diag_host.c
stackdump.c
endif
include $(AVERSIVE_DIR)/mk/aversive_module.mk
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
(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)
{
/** 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);
}
__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;
/** 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,
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;
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);
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));
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);
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,
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;
return -1;
+ }
delete_event(traj);
turnx = x + cos(a_rad) * remain;
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
(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 */
/************ 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. */
/** return true if the position consign is equal to the filtered
* position consign (after quadramp filter), for angle and
* distance. */