summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2008-05-01 14:37:03 +0200
committerNicolas Schodet2008-05-01 14:37:03 +0200
commitab9a625a4d731c4fa1dddc5143bce5014f79ec3e (patch)
tree4640c88f6feb59372afe068a6a89561c68be4d31
parentab8577a506a83095ab6e68f35bf10fbbe19341b9 (diff)
* digital/asserv/src/asserv:
- changed FTW algorithm.
-rw-r--r--digital/asserv/src/asserv/traj.c37
1 files changed, 24 insertions, 13 deletions
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index e0194ba8..810e1aa5 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -120,25 +120,33 @@ traj_ftw (void)
speed = speed_theta.slow;
speed *= 256;
speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = -speed;
+ speed_alpha.cons = 0;
+ state_main.variant = 0;
if (PINC & _BV (0) && PINC & _BV (1))
{
- speed_theta.cons = -speed;
- speed_alpha.cons = 0;
+ /* Backward. */
}
- else if (PINC & _BV (0))
+ else if (PINC & _BV (0) || PINC & _BV (1))
{
+#ifndef HOST
+ /* No angular control. */
+ state_main.variant = 2;
+#else
+ /* On host, we must do the job. */
speed_theta.cons = -speed / 2;
- speed_alpha.cons = speed / 2;
- }
- else if (PINC & _BV (1))
- {
- speed_theta.cons = -speed / 2;
- speed_alpha.cons = -speed / 2;
+ if (PINC & _BV (0))
+ speed_alpha.cons = speed / 2;
+ else
+ speed_alpha.cons = -speed / 2;
+#endif
}
else
{
- speed_theta.cons = 0;
- speed_alpha.cons = 0;
+ /* Stay here. */
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cur;
+ speed_alpha.pos_cons = pos_alpha.cur;
speed_theta.cur = 0;
speed_alpha.cur = 0;
state_finish (&state_main);
@@ -169,10 +177,13 @@ traj_gtd (void)
}
else
{
- speed_theta.cons = 0;
- speed_alpha.cons = 0;
+ /* Stay here. */
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cur;
+ speed_alpha.pos_cons = pos_alpha.cur;
speed_theta.cur = 0;
speed_alpha.cur = 0;
+ state_main.variant = 0;
state_finish (&state_main);
traj_mode = TRAJ_DONE;
}