From 24aa32ede3298db92b4c5e0f8b92951161b21160 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 21 May 2009 05:34:17 +0200 Subject: * digital/asserv: - better blocking detection for auxiliaries. --- digital/asserv/src/asserv/aux.c | 8 +++++++- digital/asserv/src/asserv/aux.h | 2 ++ digital/asserv/src/asserv/pos.c | 3 ++- digital/asserv/src/asserv/state.h | 3 ++- digital/asserv/tools/asserv/init.py | 2 ++ 5 files changed, 15 insertions(+), 3 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index 5d6d7396..22df6a6d 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -66,10 +66,12 @@ aux_init (void) aux[0].speed = &speed_aux[0]; aux[0].zero_pin = &IO_PIN (CONTACT_AUX0_ZERO_IO); aux[0].zero_bv = IO_BV (CONTACT_AUX0_ZERO_IO); + aux[0].handle_blocking = 1; aux[1].state = &state_aux[1]; aux[1].speed = &speed_aux[1]; aux[1].zero_pin = &IO_PIN (CONTACT_AUX1_ZERO_IO); aux[1].zero_bv = IO_BV (CONTACT_AUX1_ZERO_IO); + aux[1].handle_blocking = 0; } /** Update positions. */ @@ -89,7 +91,8 @@ aux_traj_goto (struct aux_t *aux) switch (aux->traj_mode) { case AUX_TRAJ_GOTO: - if (aux->speed->pos->e_old > 500) + if (aux->speed->pos->blocked_counter + > aux->speed->pos->blocked_counter_limit) { aux->traj_mode = AUX_TRAJ_GOTO_UNBLOCK; aux->speed->pos_cons = aux->speed->pos->cur; @@ -100,6 +103,7 @@ aux_traj_goto (struct aux_t *aux) < 300) { aux->traj_mode = AUX_TRAJ_DONE; + aux->state->variant = 0; state_finish (aux->state); } break; @@ -122,6 +126,8 @@ aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq) aux->speed->pos_cons += (int16_t) (pos - aux->pos); aux->goto_pos = aux->speed->pos_cons; state_start (aux->state, MODE_TRAJ, seq); + if (aux->handle_blocking) + aux->state->variant = 4; } /** Find zero mode. */ diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h index 8102d78a..65456ec9 100644 --- a/digital/asserv/src/asserv/aux.h +++ b/digital/asserv/src/asserv/aux.h @@ -44,6 +44,8 @@ struct aux_t volatile uint8_t *zero_pin; /** Top zero port bit value. */ uint8_t zero_bv; + /** Handle blocking by aux instead of pos. */ + uint8_t handle_blocking; }; extern struct aux_t aux[AC_ASSERV_AUX_NB]; diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index dc995a16..4b223331 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -163,7 +163,8 @@ pos_update_single (struct state_t *state, struct pos_t *pos, pos->blocked_counter++; else pos->blocked_counter = 0; - if (pos->blocked_counter > pos->blocked_counter_limit) + if (!(state->variant & 4) + && pos->blocked_counter > pos->blocked_counter_limit) { /* Blocked. */ pos_reset (pos); diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h index 7967ed65..2f0cc538 100644 --- a/digital/asserv/src/asserv/state.h +++ b/digital/asserv/src/asserv/state.h @@ -55,7 +55,8 @@ struct state_t /** Control mode variant. * Used for main motors: * - bit 0: disable theta position control. - * - bit 1: disable alpha position control. */ + * - bit 1: disable alpha position control. + * - bit 2: disable blocking detection. */ uint8_t variant; /** Sequence number of the currently processed command, should be between * 1 and 127. When a command is received on the serial port it is ignored diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index ba7c86e3..4415a44b 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -20,8 +20,10 @@ target = dict ( aa = 0.5, asm = 0x30, ass = 0x10, a0kp = 4, a0a = 0.5, a0sm = 0x0c, a0ss = 0x05, + a0be = 256, a0bs = 0x04, a0bc = 10, a1kp = 2, a1a = 16, a1sm = 0x46, a1ss = 0x10, + a1be = 256, a1bs = 0x0c, a1bc = 10, E = 0x3ff, D = 0x1ff, w = 0x08, ) -- cgit v1.2.3