summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/aux.c20
-rw-r--r--digital/asserv/src/asserv/aux.h3
2 files changed, 23 insertions, 0 deletions
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index 9b730a92..dd779de8 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -53,6 +53,8 @@ enum
AUX_TRAJ_GOTO_UNBLOCK,
/* Find zero mode, turn until zero is not seen. */
AUX_TRAJ_FIND_ZERO_NOT,
+ /* Find zero mode, turn until zero is not seen, then reverse. */
+ AUX_TRAJ_FIND_ZERO_NOT_REVERSE,
/* Find zero mode, turn until zero is seen. */
AUX_TRAJ_FIND_ZERO,
/* Find zero by forcing into limit. */
@@ -146,6 +148,13 @@ aux_traj_find_zero (struct aux_t *aux)
if (zero)
aux->traj_mode = AUX_TRAJ_FIND_ZERO;
break;
+ case AUX_TRAJ_FIND_ZERO_NOT_REVERSE:
+ if (zero)
+ {
+ aux->speed->cons = -aux->speed->cons;
+ aux->traj_mode = AUX_TRAJ_FIND_ZERO;
+ }
+ break;
case AUX_TRAJ_FIND_ZERO:
if (!zero)
{
@@ -168,6 +177,16 @@ aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq)
state_start (aux->state, MODE_TRAJ, seq);
}
+/** Start find zero reverse mode. */
+void
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+{
+ 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);
+}
+
/** Find limit mode. */
void
aux_traj_find_limit (struct aux_t *aux)
@@ -209,6 +228,7 @@ aux_traj_update_single (struct aux_t *aux)
aux_traj_goto (aux);
break;
case AUX_TRAJ_FIND_ZERO_NOT:
+ case AUX_TRAJ_FIND_ZERO_NOT_REVERSE:
case AUX_TRAJ_FIND_ZERO:
aux_traj_find_zero (aux);
break;
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index b541f3eb..167f880e 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -65,6 +65,9 @@ void
aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq);
void
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed, uint8_t seq);
+
+void
aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq);
void