summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2009-05-19 20:04:17 +0200
committerNicolas Schodet2009-05-19 20:04:17 +0200
commitdbb94e5c0018371838914f62bfa14a62a1ad922a (patch)
treebd7f0f1974d4aa9718590c167762b4c1c66f6ce8 /digital/asserv/src/asserv
parent1e8fee2bf4e410a64401e30e6cbca526fba20209 (diff)
* digital/asserv, digital/io:
- added backward movements.
Diffstat (limited to 'digital/asserv/src/asserv')
-rw-r--r--digital/asserv/src/asserv/main.c38
-rw-r--r--digital/asserv/src/asserv/traj.c20
-rw-r--r--digital/asserv/src/asserv/traj.h7
-rw-r--r--digital/asserv/src/asserv/twi_proto.c32
4 files changed, 33 insertions, 64 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index b34ed364..b53e6d18 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -398,27 +398,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
traj_gtd_start (args[0]);
break;
- case c ('x', 9):
+ case c ('x', 10):
/* Go to position.
* - d: x, f24.8.
* - d: y, f24.8.
+ * - b: backward (see traj.h).
* - 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]),
- 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)
+ if (args[9] == 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]);
+ args[8], args[9]);
break;
case c ('x', 3):
/* Go to angle.
@@ -429,31 +419,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
args[2]);
break;
- case c ('x', 11):
+ case c ('x', 12):
/* Go to position, then angle.
* - d: x, f24.8.
* - d: y, f24.8.
* - w: a, f0.16.
+ * - b: backward (see traj.h).
* - 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),
- 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)
+ if (args[11] == 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]);
+ args[10], args[11]);
break;
case c ('y', 4):
/* Auxiliary go to position.
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 92ff94f1..bcd99256 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -93,9 +93,6 @@ 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;
-
/** Initialise computed factors. */
void
traj_init (void)
@@ -238,7 +235,14 @@ 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 (traj_backward & TRAJ_BACKWARD)
+ {
+ if (arad > 0)
+ arad = - PI_F824 + arad;
+ else
+ arad = PI_F824 + arad;
+ }
+ if (traj_backward & TRAJ_REVERT_OK)
{
if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD)
arad = - PI_F824 + arad;
@@ -263,12 +267,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 backward_ok, uint8_t seq)
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq)
{
traj_mode = TRAJ_GOTO;
traj_goto_x = x;
traj_goto_y = y;
- traj_backward_ok = backward_ok;
+ traj_backward = backward;
speed_theta.use_pos = speed_alpha.use_pos = 1;
speed_theta.pos_cons = pos_theta.cons;
speed_alpha.pos_cons = pos_alpha.cons;
@@ -332,14 +336,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 backward_ok,
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
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;
+ traj_backward = backward;
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 c360ea2e..8f9c28ac 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -25,6 +25,9 @@
*
* }}} */
+#define TRAJ_BACKWARD 1
+#define TRAJ_REVERT_OK 2
+
extern uint8_t traj_mode;
extern int16_t traj_eps;
extern int16_t traj_aeps;
@@ -46,13 +49,13 @@ void
traj_gtd_start (uint8_t seq);
void
-traj_goto_start (uint32_t x, uint32_t y, uint8_t backward_ok, uint8_t seq);
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, 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 backward_ok,
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
uint8_t seq);
void
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index df41efaa..dac59f93 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -172,46 +172,30 @@ twi_proto_callback (u8 *buf, u8 size)
/* Go to the dispenser. */
traj_gtd_start (0);
break;
- case c ('x', 6):
+ case c ('x', 7):
/* Go to position.
* - 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),
- 0, 0);
- break;
- case c ('r', 6):
- /* Go to position, backward allowed.
- * - 3b: x position.
- * - 3b: y position. */
+ * - 3b: y position.
+ * - b: backward (see traj.h). */
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);
+ buf[8], 0);
break;
case c ('y', 2):
/* Go to angle.
* - w: angle. */
traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0);
break;
- case c ('X', 8):
+ case c ('X', 9):
/* Go to position, then angle.
* - 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),
- 0, 0);
- break;
- case c ('R', 8):
- /* Go to position, then angle, backward allowed.
- * - 3b: x position.
- * - 3b: y position.
- * - w: angle. */
+ * - w: angle.
+ * - b: backward (see traj.h). */
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);
+ buf[10], 0);
break;
case c ('b', 3):
/* Move the aux0.