summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/aux.c127
-rw-r--r--digital/asserv/src/asserv/aux.h15
-rw-r--r--digital/asserv/src/asserv/main.c8
-rw-r--r--digital/asserv/src/asserv/twi_proto.c4
4 files changed, 94 insertions, 60 deletions
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. */