summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2009-05-21 05:34:17 +0200
committerNicolas Schodet2009-05-21 05:34:17 +0200
commit24aa32ede3298db92b4c5e0f8b92951161b21160 (patch)
treeed71d08392f45cee43f9ea81e33f8ca714d25379
parent18b32888e823fef06f9e9e35895d796611fa3e8c (diff)
* digital/asserv:
- better blocking detection for auxiliaries.
-rw-r--r--digital/asserv/src/asserv/aux.c8
-rw-r--r--digital/asserv/src/asserv/aux.h2
-rw-r--r--digital/asserv/src/asserv/pos.c3
-rw-r--r--digital/asserv/src/asserv/state.h3
-rw-r--r--digital/asserv/tools/asserv/init.py2
5 files changed, 15 insertions, 3 deletions
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,
)