summaryrefslogtreecommitdiff
path: root/digital/asserv
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
parentbe1feaa0f9d7049efbeceb970ad6d7cc39988a68 (diff)
* digital/asserv/src/asserv:
- added backward goto commands.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/main.c28
-rw-r--r--digital/asserv/src/asserv/traj.c29
-rw-r--r--digital/asserv/src/asserv/traj.h5
-rw-r--r--digital/asserv/src/asserv/twi_proto.c22
-rw-r--r--digital/asserv/tools/asserv.py10
5 files changed, 79 insertions, 15 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 47b2af2f..19fe2056 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -363,7 +363,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
v8_to_v32 (args[4], args[5], args[6], args[7]),
- args[8]);
+ 0, args[8]);
+ break;
+ case c ('r', 9):
+ /* Go to position, backward allowed.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - b: sequence number. */
+ if (args[8] == state_main.sequence)
+ break;
+ traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]),
+ 1, args[8]);
break;
case c ('x', 3):
/* Go to angle.
@@ -385,7 +396,20 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
v8_to_v32 (args[4], args[5], args[6], args[7]),
v8_to_v32 (0, args[8], args[9], 0),
- args[10]);
+ 0, args[10]);
+ break;
+ case c ('r', 11):
+ /* Go to position, then angle, backward allowed.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - w: a, f0.16.
+ * - b: sequence number. */
+ if (args[10] == state_main.sequence)
+ break;
+ traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]),
+ v8_to_v32 (0, args[8], args[9], 0),
+ 1, args[10]);
break;
case c ('y', 3):
/* Auxiliary go to position.
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;
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index 5d09f082..8619f0bd 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -42,12 +42,13 @@ void
traj_gtd_start (uint8_t seq);
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);
void
traj_goto_angle_start (uint32_t a, uint8_t seq);
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);
#endif /* traj_h */
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 85ad6d2c..76639064 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -149,7 +149,15 @@ twi_proto_callback (u8 *buf, u8 size)
* - 3b: y position. */
traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
v8_to_v32 (buf[5], buf[6], buf[7], 0),
- 0);
+ 0, 0);
+ break;
+ case c ('r', 6):
+ /* Go to position, backward allowed.
+ * - 3b: x position.
+ * - 3b: y position. */
+ traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
+ v8_to_v32 (buf[5], buf[6], buf[7], 0),
+ 1, 0);
break;
case c ('y', 2):
/* Go to angle.
@@ -164,7 +172,17 @@ twi_proto_callback (u8 *buf, u8 size)
traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
v8_to_v32 (buf[5], buf[6], buf[7], 0),
v8_to_v32 (0, buf[8], buf[9], 0),
- 0);
+ 0, 0);
+ break;
+ case c ('R', 8):
+ /* Go to position, then angle, backward allowed.
+ * - 3b: x position.
+ * - 3b: y position.
+ * - w: angle. */
+ traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
+ v8_to_v32 (buf[5], buf[6], buf[7], 0),
+ v8_to_v32 (0, buf[8], buf[9], 0),
+ 1, 0);
break;
case c ('b', 3):
/* Move the arm.
diff --git a/digital/asserv/tools/asserv.py b/digital/asserv/tools/asserv.py
index 6260f0ef..e1bfa5e9 100644
--- a/digital/asserv/tools/asserv.py
+++ b/digital/asserv/tools/asserv.py
@@ -135,10 +135,11 @@ class Asserv:
self.proto.send ('s', 'LB', offset, self.seq)
self.wait (self.finished)
- def goto (self, x, y):
+ def goto (self, x, y, backward_ok = False):
"""Go to position."""
self.seq += 1
- self.proto.send ('x', 'LLB', 256 * x / self.param['scale'],
+ self.proto.send (backward_ok and 'r' or 'x', 'LLB',
+ 256 * x / self.param['scale'],
256 * y / self.param['scale'], self.seq)
self.wait (self.finished)
@@ -148,10 +149,11 @@ class Asserv:
self.proto.send ('x', 'HB', a * (1 << 16) / 360, self.seq)
self.wait (self.finished)
- def goto_xya (self, x, y, a):
+ def goto_xya (self, x, y, a, backward_ok = False):
"""Go to position, then angle."""
self.seq += 1
- self.proto.send ('x', 'LLHB', 256 * x / self.param['scale'],
+ self.proto.send (backward_ok and 'r' or 'x', 'LLHB',
+ 256 * x / self.param['scale'],
256 * y / self.param['scale'],
a * (1 << 16) / 360, self.seq)
self.wait (self.finished)