summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/aux.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv/aux.c')
-rw-r--r--digital/asserv/src/asserv/aux.c127
1 files changed, 75 insertions, 52 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);
}
+