From ab9a625a4d731c4fa1dddc5143bce5014f79ec3e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 1 May 2008 14:37:03 +0200 Subject: * digital/asserv/src/asserv: - changed FTW algorithm. --- digital/asserv/src/asserv/traj.c | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) (limited to 'digital/asserv') 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; } -- cgit v1.2.3