From 82c72072818fc6a100ba0fcce6ea816baddd9a78 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 21 Apr 2008 00:38:15 +0200 Subject: * digital/asserv/src/asserv: - fixed partial motor control. --- digital/asserv/src/asserv/pos.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index 49c7f7eb..2d8c1c18 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -99,10 +99,12 @@ pos_update (void) int16_t pid_theta, pid_alpha; int32_t diff_theta, diff_alpha; /* Update current shaft positions. */ - if (!(state_main.variant & 1)) - pos_theta.cur += counter_left_diff + counter_right_diff; - if (!(state_main.variant & 2)) - pos_alpha.cur += counter_right_diff - counter_left_diff; + pos_theta.cur += counter_left_diff + counter_right_diff; + pos_alpha.cur += counter_right_diff - counter_left_diff; + if (state_main.variant & 1) + pos_reset (&pos_theta); + if (state_main.variant & 2) + pos_reset (&pos_alpha); /* Compute PID. */ diff_theta = pos_theta.cons - pos_theta.cur; diff_alpha = pos_alpha.cons - pos_alpha.cur; -- cgit v1.2.3