From c6367a657f9326cf590c721f4c9ab4b7fcdbc59b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 9 May 2009 01:37:00 +0200 Subject: * digital/asserv: - simplified find zero. --- digital/asserv/src/asserv/aux.c | 49 ++++++++--------------------------- digital/asserv/src/asserv/aux.h | 2 +- digital/asserv/src/asserv/main.c | 9 ++++--- digital/asserv/src/asserv/twi_proto.c | 6 ++--- 4 files changed, 19 insertions(+), 47 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index f7c48b7a..787a33c5 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -50,12 +50,8 @@ enum AUX_TRAJ_GOTO, /* Goto position, try to unblock. */ AUX_TRAJ_GOTO_UNBLOCK, - /* Find zero mode, first start at full speed to detect a arm... */ - AUX_TRAJ_FIND_ZERO_START, - /* ...then go on until it is not seen any more... */ - AUX_TRAJ_FIND_ZERO_SLOW, - /* ...finally, go backward until it is seen. */ - AUX_TRAJ_FIND_ZERO_BACK, + /* Find zero mode, turn until zero is seen. */ + AUX_TRAJ_FIND_ZERO, /* Everything done. */ AUX_TRAJ_DONE, }; @@ -131,43 +127,22 @@ void aux_traj_find_zero (struct aux_t *aux) { uint8_t zero = *aux->zero_pin & aux->zero_bv; - switch (aux->traj_mode) + if (!zero) { - case AUX_TRAJ_FIND_ZERO_START: - if (!zero) - { - aux->speed->cons = aux->speed->max << 8; - aux->traj_mode = AUX_TRAJ_FIND_ZERO_SLOW; - } - break; - case AUX_TRAJ_FIND_ZERO_SLOW: - if (zero) - { - aux->speed->cons = -aux->speed->slow << 8; - aux->traj_mode = AUX_TRAJ_FIND_ZERO_BACK; - } - break; - case AUX_TRAJ_FIND_ZERO_BACK: - if (!zero) - { - aux->speed->cons = 0; - state_finish (aux->state); - aux->pos = 0; - aux->traj_mode = AUX_TRAJ_DONE; - } - break; - default: - assert (0); + aux->speed->cons = 0; + state_finish (aux->state); + aux->pos = 0; + aux->traj_mode = AUX_TRAJ_DONE; } } /** Start find zero mode. */ void -aux_traj_find_zero_start (struct aux_t *aux, uint8_t seq) +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq) { - aux->traj_mode = AUX_TRAJ_FIND_ZERO_START; + aux->traj_mode = AUX_TRAJ_FIND_ZERO; aux->speed->use_pos = 0; - aux->speed->cons = aux->speed->max << 8; + aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); } @@ -183,9 +158,7 @@ aux_traj_update_single (struct aux_t *aux) case AUX_TRAJ_GOTO_UNBLOCK: aux_traj_goto (aux); break; - case AUX_TRAJ_FIND_ZERO_START: - case AUX_TRAJ_FIND_ZERO_SLOW: - case AUX_TRAJ_FIND_ZERO_BACK: + case AUX_TRAJ_FIND_ZERO: aux_traj_find_zero (aux); break; case AUX_TRAJ_DONE: diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h index 5a95bdaf..8102d78a 100644 --- a/digital/asserv/src/asserv/aux.h +++ b/digital/asserv/src/asserv/aux.h @@ -58,7 +58,7 @@ void aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq); void -aux_traj_find_zero_start (struct aux_t *aux, uint8_t seq); +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq); void aux_traj_update (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 983995f5..f0b7e5b3 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -455,7 +455,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v32 (0, args[8], args[9], 0), 1, args[10]); break; - case c ('y', 3): + case c ('y', 4): /* Auxiliary go to position. * - b: aux index. * - w: pos, i16. @@ -465,14 +465,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]); break; - case c ('y', 1): + case c ('y', 3): /* Auxiliary find zero. * - b: aux index. + * - b: speed. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[1] == state->sequence) + if (args[2] == state->sequence) break; - aux_traj_find_zero_start (auxp, args[1]); + aux_traj_find_zero_start (auxp, args[1], args[2]); break; case c ('a', 3): /* Set all acknoledge. diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 10c49eeb..735430a2 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -202,8 +202,7 @@ twi_proto_callback (u8 *buf, u8 size) case c ('B', 1): /* Find the zero position of the arm. * - b: speed. */ - speed_aux[0].max = buf[2]; - aux_traj_find_zero_start (&aux[0], 0); + aux_traj_find_zero_start (&aux[0], buf[2], 0); break; case c ('c', 3): /* Move the aux1. @@ -215,8 +214,7 @@ twi_proto_callback (u8 *buf, u8 size) case c ('C', 1): /* Find the zero position of the aux1. * - b: speed. */ - speed_aux[1].max = buf[2]; - aux_traj_find_zero_start (&aux[1], 0); + aux_traj_find_zero_start (&aux[1], buf[2], 0); break; case c ('p', x): /* Set parameters. */ -- cgit v1.2.3