From 21a6726ab5d767aa389a8a6c8f874ccda008e607 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 Mar 2009 00:20:17 +0200 Subject: * digital/asserv/src/asserv: - use pointers for aux. --- digital/asserv/src/asserv/aux.c | 127 ++++++++++++++++++++++++---------------- 1 file changed, 75 insertions(+), 52 deletions(-) (limited to 'digital/asserv/src/asserv/aux.c') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index aa88d91e..05ada060 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -58,6 +58,16 @@ enum AUX_TRAJ_DONE, }; +/** Initialise motors states. */ +void +aux_init (void) +{ + aux0.state = &state_aux0; + aux0.speed = &speed_aux0; + aux0.zero_pin = &PINC; + aux0.zero_bv = _BV (5); +} + /** Update positions. */ void aux_pos_update (void) @@ -68,72 +78,74 @@ aux_pos_update (void) /** Goto position. */ void -aux_traj_goto (void) +aux_traj_goto (struct aux_t *aux) { - switch (aux0.traj_mode) + switch (aux->traj_mode) { case AUX_TRAJ_GOTO: - if (pos_aux0.e_old > 500) + if (aux->speed->pos->e_old > 500) { - aux0.traj_mode = AUX_TRAJ_GOTO_UNBLOCK; - speed_aux0.pos_cons = pos_aux0.cur; - speed_aux0.pos_cons -= 250; - aux0.wait = 225 / 2; + aux->traj_mode = AUX_TRAJ_GOTO_UNBLOCK; + aux->speed->pos_cons = aux->speed->pos->cur; + aux->speed->pos_cons -= 250; + aux->wait = 225 / 2; } - else if (UTILS_ABS (speed_aux0.pos_cons - pos_aux0.cur) < 300) + else if (UTILS_ABS (aux->speed->pos_cons - aux->speed->pos->cur) + < 300) { - aux0.traj_mode = AUX_TRAJ_DONE; - state_finish (&state_aux0); + aux->traj_mode = AUX_TRAJ_DONE; + state_finish (aux->state); } break; case AUX_TRAJ_GOTO_UNBLOCK: - if (!--aux0.wait) + if (!--aux->wait) { - aux0.traj_mode = AUX_TRAJ_GOTO; - speed_aux0.pos_cons = aux0.goto_pos; + aux->traj_mode = AUX_TRAJ_GOTO; + aux->speed->pos_cons = aux->goto_pos; } break; } } void -aux_traj_goto_start (uint16_t pos, uint8_t seq) +aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq) { - aux0.traj_mode = AUX_TRAJ_GOTO; - speed_aux0.use_pos = 1; - speed_aux0.pos_cons = pos_aux0.cur; - speed_aux0.pos_cons += (int16_t) (pos - aux0.pos); - aux0.goto_pos = speed_aux0.pos_cons; - state_start (&state_aux0, MODE_TRAJ, seq); + aux->traj_mode = AUX_TRAJ_GOTO; + aux->speed->use_pos = 1; + aux->speed->pos_cons = aux->speed->pos->cur; + aux->speed->pos_cons += (int16_t) (pos - aux->pos); + aux->goto_pos = aux->speed->pos_cons; + state_start (aux->state, MODE_TRAJ, seq); } /** Find zero mode. */ void -aux_traj_find_zero (void) +aux_traj_find_zero (struct aux_t *aux) { - switch (aux0.traj_mode) + uint8_t zero = *aux->zero_pin & aux->zero_bv; + switch (aux->traj_mode) { case AUX_TRAJ_FIND_ZERO_START: - if (!(PINC & _BV (5))) + if (!zero) { - speed_aux0.cons = speed_aux0.max << 8; - aux0.traj_mode = AUX_TRAJ_FIND_ZERO_SLOW; + aux->speed->cons = aux->speed->max << 8; + aux->traj_mode = AUX_TRAJ_FIND_ZERO_SLOW; } break; case AUX_TRAJ_FIND_ZERO_SLOW: - if (PINC & _BV (5)) + if (zero) { - speed_aux0.cons = -speed_aux0.slow << 8; - aux0.traj_mode = AUX_TRAJ_FIND_ZERO_BACK; + aux->speed->cons = -aux->speed->slow << 8; + aux->traj_mode = AUX_TRAJ_FIND_ZERO_BACK; } break; case AUX_TRAJ_FIND_ZERO_BACK: - if (!(PINC & _BV (5))) + if (!zero) { - speed_aux0.cons = 0; - state_finish (&state_aux0); - aux0.pos = 0; - aux0.traj_mode = AUX_TRAJ_DONE; + aux->speed->cons = 0; + state_finish (aux->state); + aux->pos = 0; + aux->traj_mode = AUX_TRAJ_DONE; } break; default: @@ -143,30 +155,41 @@ aux_traj_find_zero (void) /** Start find zero mode. */ void -aux_traj_find_zero_start (uint8_t seq) +aux_traj_find_zero_start (struct aux_t *aux, uint8_t seq) { - aux0.traj_mode = AUX_TRAJ_FIND_ZERO_START; - speed_aux0.use_pos = 0; - speed_aux0.cons = speed_aux0.max << 8; - state_start (&state_aux0, MODE_TRAJ, seq); + aux->traj_mode = AUX_TRAJ_FIND_ZERO_START; + aux->speed->use_pos = 0; + aux->speed->cons = aux->speed->max << 8; + state_start (aux->state, MODE_TRAJ, seq); +} + +/** Update trajectories for one motor. */ +static void +aux_traj_update_single (struct aux_t *aux) +{ + if (aux->state->mode >= MODE_TRAJ) + { + switch (aux->traj_mode) + { + case AUX_TRAJ_GOTO: + 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: + aux_traj_find_zero (aux); + break; + case AUX_TRAJ_DONE: + break; + } + } } /** Update trajectories. */ void aux_traj_update (void) { - switch (aux0.traj_mode) - { - case AUX_TRAJ_GOTO: - case AUX_TRAJ_GOTO_UNBLOCK: - aux_traj_goto (); - break; - case AUX_TRAJ_FIND_ZERO_START: - case AUX_TRAJ_FIND_ZERO_SLOW: - case AUX_TRAJ_FIND_ZERO_BACK: - aux_traj_find_zero (); - break; - case AUX_TRAJ_DONE: - break; - } + aux_traj_update_single (&aux0); } + -- cgit v1.2.3