summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-20 19:18:20 +0200
committerNicolas Schodet2008-04-20 19:18:20 +0200
commit0f5fb480d954339158033be901a19f15c3815847 (patch)
treede10f60e720d642d48afde6416b08246fc1a9a33 /digital/asserv
parent1457b88a0acdda5835e9bba8251acb1205435467 (diff)
* digital/asserv/src/asserv:
- added goto_xya command. - reduced the goto_angle proto parameter size. * digital/io/doc: - added goto_xya command. * digital/asserv/tools: - added goto_xya command.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/main.c23
-rw-r--r--digital/asserv/src/asserv/traj.c38
-rw-r--r--digital/asserv/src/asserv/traj.h3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c14
-rw-r--r--digital/asserv/tools/asserv.py10
5 files changed, 80 insertions, 8 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 0a746149..f0813fba 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -361,14 +361,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v32 (args[4], args[5], args[6], args[7]),
args[8]);
break;
- case c ('x', 5):
+ case c ('x', 3):
/* Go to angle.
- * - d: a, f8.24.
+ * - d: a, f0.16.
* - b: sequence number. */
- if (args[4] == state_main.sequence)
+ if (args[2] == state_main.sequence)
+ break;
+ traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
+ args[2]);
+ break;
+ case c ('x', 11):
+ /* Go to position, then angle.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - w: a, f0.16.
+ * - b: sequence number. */
+ if (args[10] == state_main.sequence)
break;
- traj_goto_angle_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
- args[4]);
+ 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]);
break;
case c ('a', 2):
/* Set both acknoledge.
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 243df1c8..e5281657 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -55,6 +55,8 @@ enum
TRAJ_GOTO,
/* Go to angle. */
TRAJ_GOTO_ANGLE,
+ /* Go to position, then angle. */
+ TRAJ_GOTO_XYA,
/* Everything done. */
TRAJ_DONE,
};
@@ -250,6 +252,39 @@ traj_goto_angle_start (uint32_t a, uint8_t seq)
state_start (&state_main, MODE_TRAJ, seq);
}
+/** Go to position, then angle mode. */
+static void
+traj_goto_xya (void)
+{
+ int32_t dx = traj_goto_x - postrack_x;
+ int32_t dy = traj_goto_y - postrack_y;
+ if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8
+ && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8)
+ {
+ /* Near enough, now do a go to angle. */
+ traj_mode = TRAJ_GOTO_ANGLE;
+ traj_goto_angle ();
+ }
+ else
+ {
+ traj_goto ();
+ }
+}
+
+/** 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_mode = TRAJ_GOTO_XYA;
+ traj_goto_x = x;
+ traj_goto_y = y;
+ traj_goto_a = a;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_TRAJ, seq);
+}
+
/* Compute new speed according the defined trajectory. */
void
traj_update (void)
@@ -268,6 +303,9 @@ traj_update (void)
case TRAJ_GOTO_ANGLE:
traj_goto_angle ();
break;
+ case TRAJ_GOTO_XYA:
+ traj_goto_xya ();
+ break;
case TRAJ_DONE:
break;
}
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index fe9830c9..5d09f082 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -47,4 +47,7 @@ traj_goto_start (uint32_t x, uint32_t y, 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);
+
#endif /* traj_h */
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 171a6895..c7564766 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -139,7 +139,7 @@ twi_proto_callback (u8 *buf, u8 size)
/* Go to the dispenser. */
traj_gtd_start (0);
break;
- case c ('x', 0):
+ case c ('x', 6):
/* Go to position.
* - 3b: x position.
* - 3b: y position. */
@@ -147,11 +147,21 @@ twi_proto_callback (u8 *buf, u8 size)
v8_to_v32 (buf[5], buf[6], buf[7], 0),
0);
break;
- case c ('y', 0):
+ 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):
+ /* 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);
+ break;
case c ('b', 3):
/* Move the arm.
* - w: new position.
diff --git a/digital/asserv/tools/asserv.py b/digital/asserv/tools/asserv.py
index 2687099f..6260f0ef 100644
--- a/digital/asserv/tools/asserv.py
+++ b/digital/asserv/tools/asserv.py
@@ -145,7 +145,15 @@ class Asserv:
def goto_angle (self, a):
"""Go to angle."""
self.seq += 1
- self.proto.send ('x', 'LB', a * (1 << 24) / 360, self.seq)
+ self.proto.send ('x', 'HB', a * (1 << 16) / 360, self.seq)
+ self.wait (self.finished)
+
+ def goto_xya (self, x, y, a):
+ """Go to position, then angle."""
+ self.seq += 1
+ self.proto.send ('x', 'LLHB', 256 * x / self.param['scale'],
+ 256 * y / self.param['scale'],
+ a * (1 << 16) / 360, self.seq)
self.wait (self.finished)
def send_param (self):