From 31f85163b5981a78a2b6704168d0ae12929b53be Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 09:39:16 +0200 Subject: digital/mimot: better find zero/limit --- digital/mimot/src/dirty/aux.c | 12 ++++++++---- digital/mimot/src/dirty/aux.h | 8 ++++++-- digital/mimot/src/dirty/main.c | 13 ++++++++++--- digital/mimot/src/dirty/twi_proto.c | 26 +++++++++++++++++--------- 4 files changed, 41 insertions(+), 18 deletions(-) (limited to 'digital/mimot') diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c index e00e2c08..e36489ba 100644 --- a/digital/mimot/src/dirty/aux.c +++ b/digital/mimot/src/dirty/aux.c @@ -184,7 +184,7 @@ aux_traj_find_zero (struct aux_t *aux) { aux->speed->cons = 0; state_finish (aux->state); - aux->pos = 0; + aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; } break; @@ -193,12 +193,14 @@ aux_traj_find_zero (struct aux_t *aux) /** Start find zero mode. */ void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq) +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq) { aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT; aux->speed->use_pos = 0; aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); + aux->reset_pos = reset_pos; } /** Find limit mode. */ @@ -225,7 +227,7 @@ aux_traj_find_limit (struct aux_t *aux) if (!--aux->wait) { state_finish (aux->state); - aux->pos = 0; + aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; } break; @@ -234,13 +236,15 @@ aux_traj_find_limit (struct aux_t *aux) /** Start find limit mode. */ void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq) +aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq) { aux->traj_mode = AUX_TRAJ_FIND_LIMIT; aux->speed->use_pos = 0; aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); aux->state->variant = 4; + aux->reset_pos = reset_pos; } /** Update trajectories for one motor. */ diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h index f31b789a..7ec60617 100644 --- a/digital/mimot/src/dirty/aux.h +++ b/digital/mimot/src/dirty/aux.h @@ -50,6 +50,8 @@ struct aux_t uint8_t zero_bv; /** Handle blocking by aux instead of pos. */ uint8_t handle_blocking; + /** Reset position after zero is found. */ + int16_t reset_pos; }; extern struct aux_t aux[AC_ASSERV_AUX_NB]; @@ -68,10 +70,12 @@ aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm, uint8_t seq); void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq); +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq); void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq); +aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq); void aux_traj_update (void); diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c index 5dcfd8c7..2b55259b 100644 --- a/digital/mimot/src/dirty/main.c +++ b/digital/mimot/src/dirty/main.c @@ -287,15 +287,22 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]), args[4]); break; - case c ('y', 3): + case c ('y', 6): /* Auxiliary find zero. * - b: aux index. * - b: speed. + * - b: use switch. + * - w: reset position. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[2] == state->sequence) + if (args[5] == state->sequence) break; - aux_traj_find_limit_start (auxp, args[1], args[2]); + if (args[2]) + aux_traj_find_zero_start (auxp, args[1], + v8_to_v16 (args[3], args[4]), args[5]); + else + aux_traj_find_limit_start (auxp, args[1], + v8_to_v16 (args[3], args[4]), args[5]); break; case c ('a', 2): /* Set all acknoledge. diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index bda255ef..2f935fda 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -122,11 +122,6 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux[0].max = buf[4]; aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0); break; - case c ('B', 1): - /* Find the zero position of the aux0. - * - b: speed. */ - aux_traj_find_limit_start (&aux[0], buf[2], 0); - break; case c ('c', 3): /* Move the aux1. * - w: new position. @@ -134,10 +129,23 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux[1].max = buf[4]; aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0); break; - case c ('C', 1): - /* Find the zero position of the aux1. - * - b: speed. */ - aux_traj_find_limit_start (&aux[1], buf[2], 0); + case c ('B', 5): + /* Find the zero position. + * - b: aux index. + * - b: speed. + * - b: use switch. + * - w: reset position. */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + if (buf[4]) + aux_traj_find_zero_start (&aux[buf[2]], buf[3], + v8_to_v16 (buf[5], buf[6]), 0); + else + aux_traj_find_limit_start (&aux[buf[2]], buf[3], + v8_to_v16 (buf[5], buf[6]), 0); + } + else + buf[0] = 0; break; case c ('l', 4): /* Clamp. -- cgit v1.2.3