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/ai/src/twi_master/mimot.c | 34 ++++++++++++++++++++++++++++------ digital/ai/src/twi_master/mimot.h | 10 ++++++++++ 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 +++++++++++++++++--------- 6 files changed, 79 insertions(+), 24 deletions(-) diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index c3b82823..6159e6c9 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -165,20 +165,42 @@ mimot_move_motor1_absolute (uint16_t position, uint8_t speed) void mimot_motor0_zero_position (int8_t speed) +{ + mimot_motor0_find_zero (speed, 0, 0); +} + +void +mimot_motor1_zero_position (int8_t speed) +{ + mimot_motor1_find_zero (speed, 0, 0); +} + +void +mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'B'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[1] = 0; + buffer[2] = speed; + buffer[3] = use_switch; + buffer[4] = v16_to_v8 (reset_position, 1); + buffer[5] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (6); } void -mimot_motor1_zero_position (int8_t speed) +mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); - buffer[0] = 'C'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[0] = 'B'; + buffer[1] = 1; + buffer[2] = speed; + buffer[3] = use_switch; + buffer[4] = v16_to_v8 (reset_position, 1); + buffer[5] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (6); } void diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index 97575b0d..c59c1608 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -92,6 +92,16 @@ mimot_motor0_zero_position (int8_t speed); void mimot_motor1_zero_position (int8_t speed); +/** Find zero position. */ +void +mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position); + +/** Find zero position. */ +void +mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position); + /** Clamp motor0. */ void mimot_motor0_clamp (int8_t speed, int16_t pwm); 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