summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/traj.c
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/src/asserv/traj.c
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/src/asserv/traj.c')
-rw-r--r--digital/asserv/src/asserv/traj.c38
1 files changed, 38 insertions, 0 deletions
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;
}