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 ++++++++++++++++++++-------------- digital/asserv/src/asserv/aux.h | 15 +++- digital/asserv/src/asserv/main.c | 8 +-- digital/asserv/src/asserv/twi_proto.c | 4 +- 4 files changed, 94 insertions(+), 60 deletions(-) (limited to 'digital') 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); } + diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h index b1ba847f..9460e87f 100644 --- a/digital/asserv/src/asserv/aux.h +++ b/digital/asserv/src/asserv/aux.h @@ -28,6 +28,10 @@ /** Auxiliary motor informations. */ struct aux_t { + /** Associated state. */ + struct state_t *state; + /** Controlled speed. */ + struct speed_t *speed; /** Absolute position. */ int16_t pos; /** Trajectory mode. */ @@ -36,18 +40,25 @@ struct aux_t uint32_t goto_pos; /** Wait counter. */ uint16_t wait; + /** Top zero port input register. */ + volatile uint8_t *zero_pin; + /** Top zero port bit value. */ + uint8_t zero_bv; }; extern struct aux_t aux0; +void +aux_init (void); + void aux_pos_update (void); 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); void -aux_traj_find_zero_start (uint8_t seq); +aux_traj_find_zero_start (struct aux_t *aux, 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 1ba1f956..f6e07126 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -110,6 +110,7 @@ main (int argc, char **argv) postrack_init (); speed_init (); traj_init (); + aux_init (); eeprom_read_params (); proto_send0 ('z'); sei (); @@ -139,8 +140,7 @@ main_loop (void) /* Compute trajectory. */ if (state_main.mode >= MODE_TRAJ) traj_update (); - if (state_aux0.mode >= MODE_TRAJ) - aux_traj_update (); + aux_traj_update (); /* Speed control. */ speed_update (); main_timer[3] = timer_read (); @@ -416,14 +416,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: sequence number. */ if (args[2] == state_aux0.sequence) break; - aux_traj_goto_start (v8_to_v16 (args[0], args[1]), args[2]); + aux_traj_goto_start (&aux0, v8_to_v16 (args[0], args[1]), args[2]); break; case c ('y', 1): /* Auxiliary find zero. * - b: sequence number. */ if (args[0] == state_aux0.sequence) break; - aux_traj_find_zero_start (args[0]); + aux_traj_find_zero_start (&aux0, args[0]); break; case c ('a', 2): /* Set both acknoledge. diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index c235a626..13cc441d 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -191,13 +191,13 @@ twi_proto_callback (u8 *buf, u8 size) * - w: new position. * - b: speed. */ speed_aux0.max = buf[4]; - aux_traj_goto_start (v8_to_v16 (buf[2], buf[3]), 0); + aux_traj_goto_start (&aux0, v8_to_v16 (buf[2], buf[3]), 0); break; case c ('B', 1): /* Find the zero position of the arm. * - b: speed. */ speed_aux0.max = buf[2]; - aux_traj_find_zero_start (0); + aux_traj_find_zero_start (&aux0, 0); break; case c ('p', x): /* Set parameters. */ -- cgit v1.2.3