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 ++++++++++++++++++++++++--------- 1 file changed, 24 insertions(+), 9 deletions(-) (limited to 'digital/mimot/src/dirty/aux.c') 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: -- cgit v1.2.3