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/main.c | 7 ------- digital/asserv/src/asserv/pos.c | 13 +++++++------ digital/asserv/src/asserv/state.h | 10 ++++++++-- 3 files changed, 15 insertions(+), 15 deletions(-) (limited to 'digital') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 38a2e9f8..e96ebf90 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -326,18 +326,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set both acknoledge. * - b: main ack sequence number. * - b: auxiliary ack sequence number. */ - if (state_aux0.blocked && args[1] == state_aux0.finished) - pos_reset (&pos_aux0); state_acknowledge (&state_aux0, args[1]); /* no break; */ case c ('a', 1): /* Set main acknoledge. * - b: main ack sequence number. */ - if (state_main.blocked && args[0] == state_main.finished) - { - pos_reset (&pos_theta); - pos_reset (&pos_alpha); - } state_acknowledge (&state_main, args[0]); break; /* Stats. 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 { diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h index 49ae1e60..2a5e642c 100644 --- a/digital/asserv/src/asserv/state.h +++ b/digital/asserv/src/asserv/state.h @@ -83,6 +83,7 @@ state_start (struct state_t *motor, uint8_t sequence) { motor->sequence = sequence; motor->finished = 0; + motor->blocked = 0; } /** Signal the current command completion. */ @@ -93,21 +94,26 @@ state_finish (struct state_t *motor) motor->finished = 1; } -/** Signal the current command completion. */ +/** Signal the current command is blocked, disable motor control until a new + * command is given. */ static inline void state_blocked (struct state_t *motor) { motor->sequence_finish = motor->sequence | 0x80; motor->blocked = 1; + motor->mode = MODE_PWM; } -/** Acknowledge a command completion. */ +/** Acknowledge a command completion and blocked state. */ static inline void state_acknowledge (struct state_t *motor, uint8_t sequence) { motor->sequence_ack = sequence; if (sequence == motor->sequence_finish) + { + motor->finished = 0; motor->blocked = 0; + } } #endif /* state_h */ -- cgit v1.2.3