From f3cce02b94b74c9b6af873965826a873520906dc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 10 May 2010 23:55:31 +0200 Subject: digital/mimot/src/dirty: add delay on find zero This is done so that hardware elasticity can move the actuator back. --- digital/mimot/src/dirty/aux.c | 33 ++++++++++++++++++++++++--------- digital/mimot/src/dirty/pos.c | 6 +++++- 2 files changed, 29 insertions(+), 10 deletions(-) (limited to 'digital/mimot') diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c index c11bdf7c..bf43795b 100644 --- a/digital/mimot/src/dirty/aux.c +++ b/digital/mimot/src/dirty/aux.c @@ -59,6 +59,8 @@ enum AUX_TRAJ_FIND_ZERO, /* Find zero by forcing into limit. */ AUX_TRAJ_FIND_LIMIT, + /* Wait for mechanical elasticity. */ + AUX_TRAJ_FIND_LIMIT_WAIT, /* Everything done. */ AUX_TRAJ_DONE, }; @@ -203,16 +205,28 @@ aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq) void aux_traj_find_limit (struct aux_t *aux) { - /* If blocking, limit is found. */ - if (aux->speed->pos->blocked_counter - > aux->speed->pos->blocked_counter_limit) + switch (aux->traj_mode) { - state_finish (aux->state); - pos_reset (aux->speed->pos); - aux->state->mode = MODE_PWM; - pwm_set (aux->pwm, 0); - aux->pos = 0; - aux->traj_mode = AUX_TRAJ_DONE; + case AUX_TRAJ_FIND_LIMIT: + /* If blocking, limit is found. */ + if (aux->speed->pos->blocked_counter + > aux->speed->pos->blocked_counter_limit) + { + pos_reset (aux->speed->pos); + aux->state->variant = 1; + pwm_set (aux->pwm, 0); + aux->traj_mode = AUX_TRAJ_FIND_LIMIT_WAIT; + aux->wait = 3 * 225; + } + break; + case AUX_TRAJ_FIND_LIMIT_WAIT: + if (!--aux->wait) + { + state_finish (aux->state); + aux->pos = 0; + aux->traj_mode = AUX_TRAJ_DONE; + } + break; } } @@ -247,6 +261,7 @@ aux_traj_update_single (struct aux_t *aux) aux_traj_find_zero (aux); break; case AUX_TRAJ_FIND_LIMIT: + case AUX_TRAJ_FIND_LIMIT_WAIT: aux_traj_find_limit (aux); break; case AUX_TRAJ_DONE: diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c index ef6475c2..28684414 100644 --- a/digital/mimot/src/dirty/pos.c +++ b/digital/mimot/src/dirty/pos.c @@ -103,7 +103,11 @@ pos_update_single (struct state_t *state, struct pos_t *pos, pos->blocked_counter++; else pos->blocked_counter = 0; - if (!(state->variant & 4) + if (state->variant & 1) + { + pos_reset (pos); + } + else if (!(state->variant & 4) && pos->blocked_counter > pos->blocked_counter_limit) { /* Blocked. */ -- cgit v1.2.3