From 1ea652e0d5185aee5ca60182302d7bb2d4056b46 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 25 Mar 2008 23:04:07 +0100 Subject: * digital/asserv/src/asserv: - better blocked state handling, now deactivate all motor control on blocking. --- digital/asserv/src/asserv/pos.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) (limited to 'digital/asserv/src/asserv/pos.c') diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index 091335e2..6c317766 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -101,14 +101,15 @@ pos_update (void) /* Compute PID. */ diff_theta = pos_theta.cons - pos_theta.cur; diff_alpha = pos_alpha.cons - pos_alpha.cur; - if (state_main.blocked - || diff_theta < -pos_blocked || pos_blocked < diff_theta + if (diff_theta < -pos_blocked || pos_blocked < diff_theta || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) { /* Blocked. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_blocked (&state_main); pwm_left = 0; pwm_right = 0; - state_blocked (&state_main); } else { @@ -129,12 +130,12 @@ pos_update (void) pos_aux0.cur += counter_aux0_diff; /* Compute PID. */ diff = pos_aux0.cons - pos_aux0.cur; - if (state_aux0.blocked - || diff < -pos_blocked || pos_blocked < diff) + if (diff < -pos_blocked || pos_blocked < diff) { /* Blocked. */ - pwm_aux0 = 0; + pos_reset (&pos_aux0); state_blocked (&state_aux0); + pwm_aux0 = 0; } else { -- cgit v1.2.3