summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv')
-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
4 files changed, 13 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