summaryrefslogtreecommitdiffhomepage
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.c120
1 files changed, 62 insertions, 58 deletions
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index c300e3f3..e4cce306 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -28,13 +28,6 @@
#include "modules/utils/utils.h"
#include "io.h"
-#include "state.h"
-
-#include "counter.h"
-#include "pos.h"
-#include "speed.h"
-#include "pwm.h"
-
#include "contacts.h"
#ifdef HOST
@@ -47,6 +40,10 @@ struct aux_t aux[AC_ASSERV_AUX_NB];
/** Trajectory modes. */
enum
{
+ /* Everything done. */
+ AUX_TRAJ_DONE,
+ /* Detect end of speed controled position control. */
+ AUX_TRAJ_SPEED,
/* Goto position, with blocking detection. */
AUX_TRAJ_GOTO,
/* Goto position, try to unblock. */
@@ -59,23 +56,17 @@ enum
AUX_TRAJ_FIND_ZERO,
/* Find zero by forcing into limit. */
AUX_TRAJ_FIND_LIMIT,
- /* Everything done. */
- AUX_TRAJ_DONE,
};
/** Initialise motors states. */
void
aux_init (void)
{
- aux[0].state = &state_aux[0];
- aux[0].speed = &speed_aux[0];
- aux[0].pwm = &pwm_aux[0];
+ aux[0].cs = &cs_aux[0];
aux[0].zero_pin = &IO_PIN (CONTACT_AUX0_ZERO_IO);
aux[0].zero_bv = IO_BV (CONTACT_AUX0_ZERO_IO);
aux[0].handle_blocking = 0;
- aux[1].state = &state_aux[1];
- aux[1].speed = &speed_aux[1];
- aux[1].pwm = &pwm_aux[1];
+ aux[1].cs = &cs_aux[1];
aux[1].zero_pin = &IO_PIN (CONTACT_AUX1_ZERO_IO);
aux[1].zero_bv = IO_BV (CONTACT_AUX1_ZERO_IO);
aux[1].handle_blocking = 0;
@@ -88,7 +79,28 @@ aux_pos_update (void)
uint8_t i;
/* Easy... */
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- aux[i].pos += counter_aux_diff[i];
+ aux[i].pos += aux[i].cs->encoder->diff;
+}
+
+/** Wait for zero speed mode. */
+void
+aux_traj_speed (struct aux_t *aux)
+{
+ if (aux->cs->speed.use_pos
+ && aux->cs->speed.pos_cons == aux->cs->pos.cons)
+ {
+ control_state_finished (&aux->cs->state);
+ aux->traj_mode = AUX_TRAJ_DONE;
+ }
+}
+
+/** Start speed mode. */
+void
+aux_traj_speed_start (struct aux_t *aux)
+{
+ /* Speed setup should have been done yet. */
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
+ aux->traj_mode = AUX_TRAJ_SPEED;
}
/** Goto position. */
@@ -98,43 +110,39 @@ aux_traj_goto (struct aux_t *aux)
switch (aux->traj_mode)
{
case AUX_TRAJ_GOTO:
- if (aux->speed->pos->blocked_counter
- > aux->speed->pos->blocked_counter_limit)
+ if (aux->cs->blocking_detection.blocked)
{
aux->traj_mode = AUX_TRAJ_GOTO_UNBLOCK;
- aux->speed->pos_cons = aux->speed->pos->cur;
- aux->speed->pos_cons -= 250;
+ speed_control_pos_offset_from_here (&aux->cs->speed, -250);
aux->wait = 225 / 2;
}
- else if (UTILS_ABS ((int32_t) (aux->speed->pos_cons -
- aux->speed->pos->cur)) < 300)
+ else if (UTILS_ABS ((int32_t) (aux->cs->speed.pos_cons
+ - aux->cs->pos.cur)) < 300)
{
aux->traj_mode = AUX_TRAJ_DONE;
- aux->state->variant = 0;
- state_finish (aux->state);
+ control_state_finished (&aux->cs->state);
}
break;
case AUX_TRAJ_GOTO_UNBLOCK:
if (!--aux->wait)
{
aux->traj_mode = AUX_TRAJ_GOTO;
- aux->speed->pos_cons = aux->goto_pos;
+ speed_control_pos (&aux->cs->speed, aux->goto_pos);
}
break;
}
}
void
-aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq)
+aux_traj_goto_start (struct aux_t *aux, uint16_t pos)
{
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);
- if (aux->handle_blocking)
- aux->state->variant = 4;
+ speed_control_pos_offset_from_here (&aux->cs->speed,
+ (int16_t) (pos - aux->pos));
+ aux->goto_pos = aux->cs->speed.pos_cons;
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL,
+ aux->handle_blocking ? CS_MODE_BLOCKING_DETECTION
+ : 0);
}
/** Find zero mode. */
@@ -151,15 +159,15 @@ aux_traj_find_zero (struct aux_t *aux)
case AUX_TRAJ_FIND_ZERO_NOT_REVERSE:
if (zero)
{
- aux->speed->cons = -aux->speed->cons;
+ aux->cs->speed.cons = -aux->cs->speed.cons;
aux->traj_mode = AUX_TRAJ_FIND_ZERO;
}
break;
case AUX_TRAJ_FIND_ZERO:
if (!zero)
{
- aux->speed->cons = 0;
- state_finish (aux->state);
+ speed_control_set_speed (&aux->cs->speed, 0);
+ control_state_finished (&aux->cs->state);
aux->pos = 0;
aux->traj_mode = AUX_TRAJ_DONE;
}
@@ -169,22 +177,20 @@ aux_traj_find_zero (struct aux_t *aux)
/** Start find zero mode. */
void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT;
- aux->speed->use_pos = 0;
- aux->speed->cons = speed << 8;
- state_start (aux->state, MODE_TRAJ, seq);
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Start find zero reverse mode. */
void
-aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT_REVERSE;
- aux->speed->use_pos = 0;
- aux->speed->cons = speed << 8;
- state_start (aux->state, MODE_TRAJ, seq);
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL, 0);
}
/** Find limit mode. */
@@ -192,15 +198,11 @@ void
aux_traj_find_limit (struct aux_t *aux)
{
/* If blocking, limit is found. */
- if (aux->speed->pos->blocked_counter
- > aux->speed->pos->blocked_counter_limit)
+ if (aux->cs->blocking_detection.blocked)
{
- state_finish (aux->state);
- pos_reset (aux->speed->pos);
- aux->speed->cur = 0;
- aux->state->mode = MODE_PWM;
- aux->state->variant = 0;
- pwm_set (aux->pwm, 0);
+ control_state_set_mode (&aux->cs->state, CS_MODE_NONE, 0);
+ control_state_finished (&aux->cs->state);
+ output_set (aux->cs->output, 0);
aux->pos = 0;
aux->traj_mode = AUX_TRAJ_DONE;
}
@@ -208,23 +210,25 @@ aux_traj_find_limit (struct aux_t *aux)
/** Start find limit mode. */
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+aux_traj_find_limit_start (struct aux_t *aux, int8_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_LIMIT;
- aux->speed->use_pos = 0;
- aux->speed->cons = speed << 8;
- state_start (aux->state, MODE_TRAJ, seq);
- aux->state->variant = 4;
+ speed_control_set_speed (&aux->cs->speed, speed);
+ control_state_set_mode (&aux->cs->state, CS_MODE_TRAJ_CONTROL,
+ CS_MODE_BLOCKING_DETECTION);
}
/** Update trajectories for one motor. */
static void
aux_traj_update_single (struct aux_t *aux)
{
- if (aux->state->mode >= MODE_TRAJ)
+ if (aux->cs->state.modes & CS_MODE_TRAJ_CONTROL)
{
switch (aux->traj_mode)
{
+ case AUX_TRAJ_SPEED:
+ aux_traj_speed (aux);
+ break;
case AUX_TRAJ_GOTO:
case AUX_TRAJ_GOTO_UNBLOCK:
aux_traj_goto (aux);