summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv/traj.c
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-23 21:02:33 +0200
committerNicolas Schodet2008-04-23 21:02:33 +0200
commitb0e46d53dacdcc37dd4379c32fe886e5776f76c7 (patch)
treeb579095b3173c1fd925973f1335cacdb06a90dc0 /digital/asserv/src/asserv/traj.c
parentbe1feaa0f9d7049efbeceb970ad6d7cc39988a68 (diff)
* digital/asserv/src/asserv:
- added backward goto commands.
Diffstat (limited to 'digital/asserv/src/asserv/traj.c')
-rw-r--r--digital/asserv/src/asserv/traj.c29
1 files changed, 24 insertions, 5 deletions
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index e5281657..cdac0538 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -44,6 +44,12 @@
/* This file provides high level trajectories commands. */
+/** Angle after which a backward movement is considered. */
+#define TRAJ_GOTO_BACKWARD_THRESHOLD ((int32_t) (0.55 * M_PI * (1L << 24)))
+
+/** The famous number. */
+#define PI_F824 ((int32_t) (M_PI * (1L << 24)))
+
/** Traj mode enum. */
enum
{
@@ -76,11 +82,14 @@ static uint32_t traj_goto_x, traj_goto_y;
/** Go to angle. */
static uint32_t traj_goto_a;
+/** Allow backward movements. */
+static uint8_t traj_backward_ok;
+
/** Angle offset. Directly handled to speed layer. */
void
traj_angle_offset_start (int32_t angle, uint8_t seq)
{
- int32_t a = fixed_mul_f824 (angle, 2 * M_PI * (1L << 24));
+ int32_t a = fixed_mul_f824 (angle, 2 * PI_F824);
uint32_t f = postrack_footing;
int32_t arc = fixed_mul_f824 (f, a);
speed_theta.use_pos = speed_alpha.use_pos = 1;
@@ -187,11 +196,18 @@ traj_goto (void)
int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s);
/* Compute arc length. */
int32_t arad = atan2 (da, dt) * (1L << 24);
+ if (traj_backward_ok)
+ {
+ if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD)
+ arad = - PI_F824 + arad;
+ else if (arad < - TRAJ_GOTO_BACKWARD_THRESHOLD)
+ arad = PI_F824 + arad;
+ }
int32_t arc = fixed_mul_f824 (arad, postrack_footing);
/* Compute consign. */
speed_alpha.pos_cons = pos_alpha.cur;
speed_alpha.pos_cons += arc;
- if (UTILS_ABS (arad) < 0.5 * M_PI * (1L << 24))
+ if (UTILS_ABS (arad) < PI_F824 / 2)
{
speed_theta.pos_cons = pos_theta.cur;
speed_theta.pos_cons += dt >> 8;
@@ -205,11 +221,12 @@ traj_goto (void)
/** Start go to position mode (x, y: f24.8). */
void
-traj_goto_start (uint32_t x, uint32_t y, uint8_t seq)
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward_ok, uint8_t seq)
{
traj_mode = TRAJ_GOTO;
traj_goto_x = x;
traj_goto_y = y;
+ traj_backward_ok = backward_ok;
speed_theta.use_pos = speed_alpha.use_pos = 1;
speed_theta.pos_cons = pos_theta.cons;
speed_alpha.pos_cons = pos_alpha.cons;
@@ -232,7 +249,7 @@ traj_goto_angle (void)
{
/* Compute arc length. */
int32_t arad = fixed_mul_f824 (((int32_t) da) << 8,
- 2 * M_PI * (1L << 24));
+ 2 * PI_F824);
int32_t arc = fixed_mul_f824 (arad, postrack_footing);
/* Compute consign. */
speed_alpha.pos_cons = pos_alpha.cur;
@@ -273,12 +290,14 @@ traj_goto_xya (void)
/** Start go to position, then angle mode (x, y: f24.8, a: f8.24). */
void
-traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t seq)
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward_ok,
+ uint8_t seq)
{
traj_mode = TRAJ_GOTO_XYA;
traj_goto_x = x;
traj_goto_y = y;
traj_goto_a = a;
+ traj_backward_ok = backward_ok;
speed_theta.use_pos = speed_alpha.use_pos = 1;
speed_theta.pos_cons = pos_theta.cons;
speed_alpha.pos_cons = pos_alpha.cons;