summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/traj.c
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-06 23:22:14 +0200
committerNicolas Schodet2008-04-06 23:22:14 +0200
commit4a78f478a8787f255b782f077b6931cbd51af1d3 (patch)
tree90fdc1a93dd3a8ba8322de6f0e0b12c89b6618b5 /digital/asserv/src/asserv/traj.c
parentfcee99a4ff34939a6dbde8238e660f5e70f8d8cd (diff)
* digital/asserv/src/asserv:
- added go to the dispenser.
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 d33c76c9..ce8a7585 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -46,6 +46,8 @@ enum
{
/* Go to the wall. */
TRAJ_FTW,
+ /* Go to the dispenser. */
+ TRAJ_GTD,
/* Everything done. */
TRAJ_DONE,
};
@@ -111,6 +113,39 @@ traj_ftw_start (uint8_t seq)
state_start (&state_main, seq);
}
+/** Go to the dispenser mode. */
+static void
+traj_gtd (void)
+{
+ int16_t speed;
+ speed = speed_theta.slow;
+ speed *= 256;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ if (PINC & _BV (2))
+ {
+ speed_theta.cons = speed;
+ speed_alpha.cons = 0;
+ }
+ else
+ {
+ speed_theta.cons = 0;
+ speed_alpha.cons = 0;
+ speed_theta.cur = 0;
+ speed_alpha.cur = 0;
+ state_finish (&state_main);
+ traj_mode = TRAJ_DONE;
+ }
+}
+
+/** Start go to the dispenser mode. */
+void
+traj_gtd_start (uint8_t seq)
+{
+ state_main.mode = MODE_TRAJ;
+ traj_mode = TRAJ_GTD;
+ state_start (&state_main, seq);
+}
+
/* Compute new speed according the defined trajectory. */
void
traj_update (void)
@@ -120,6 +155,9 @@ traj_update (void)
case TRAJ_FTW:
traj_ftw ();
break;
+ case TRAJ_GTD:
+ traj_gtd ();
+ break;
case TRAJ_DONE:
break;
}