From 4a78f478a8787f255b782f077b6931cbd51af1d3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 6 Apr 2008 23:22:14 +0200 Subject: * digital/asserv/src/asserv: - added go to the dispenser. --- digital/asserv/src/asserv/traj.c | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) (limited to 'digital/asserv/src/asserv/traj.c') 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; } -- cgit v1.2.3