summaryrefslogtreecommitdiffhomepage
path: root/digital/mimot
diff options
context:
space:
mode:
Diffstat (limited to 'digital/mimot')
-rw-r--r--digital/mimot/src/dirty/aux.c12
-rw-r--r--digital/mimot/src/dirty/aux.h8
-rw-r--r--digital/mimot/src/dirty/main.c13
-rw-r--r--digital/mimot/src/dirty/twi_proto.c26
4 files changed, 41 insertions, 18 deletions
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
index e00e2c08..e36489ba 100644
--- a/digital/mimot/src/dirty/aux.c
+++ b/digital/mimot/src/dirty/aux.c
@@ -184,7 +184,7 @@ aux_traj_find_zero (struct aux_t *aux)
{
aux->speed->cons = 0;
state_finish (aux->state);
- aux->pos = 0;
+ aux->pos = aux->reset_pos;
aux->traj_mode = AUX_TRAJ_DONE;
}
break;
@@ -193,12 +193,14 @@ 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, int16_t reset_pos,
+ uint8_t seq)
{
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);
+ aux->reset_pos = reset_pos;
}
/** Find limit mode. */
@@ -225,7 +227,7 @@ aux_traj_find_limit (struct aux_t *aux)
if (!--aux->wait)
{
state_finish (aux->state);
- aux->pos = 0;
+ aux->pos = aux->reset_pos;
aux->traj_mode = AUX_TRAJ_DONE;
}
break;
@@ -234,13 +236,15 @@ 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, int16_t reset_pos,
+ uint8_t seq)
{
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;
+ aux->reset_pos = reset_pos;
}
/** Update trajectories for one motor. */
diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h
index f31b789a..7ec60617 100644
--- a/digital/mimot/src/dirty/aux.h
+++ b/digital/mimot/src/dirty/aux.h
@@ -50,6 +50,8 @@ struct aux_t
uint8_t zero_bv;
/** Handle blocking by aux instead of pos. */
uint8_t handle_blocking;
+ /** Reset position after zero is found. */
+ int16_t reset_pos;
};
extern struct aux_t aux[AC_ASSERV_AUX_NB];
@@ -68,10 +70,12 @@ aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm,
uint8_t seq);
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, int16_t reset_pos,
+ uint8_t seq);
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, int16_t reset_pos,
+ uint8_t seq);
void
aux_traj_update (void);
diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
index 5dcfd8c7..2b55259b 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -287,15 +287,22 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]),
args[4]);
break;
- case c ('y', 3):
+ case c ('y', 6):
/* Auxiliary find zero.
* - b: aux index.
* - b: speed.
+ * - b: use switch.
+ * - w: reset position.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[2] == state->sequence)
+ if (args[5] == state->sequence)
break;
- aux_traj_find_limit_start (auxp, args[1], args[2]);
+ if (args[2])
+ aux_traj_find_zero_start (auxp, args[1],
+ v8_to_v16 (args[3], args[4]), args[5]);
+ else
+ aux_traj_find_limit_start (auxp, args[1],
+ v8_to_v16 (args[3], args[4]), args[5]);
break;
case c ('a', 2):
/* Set all acknoledge.
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index bda255ef..2f935fda 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -122,11 +122,6 @@ twi_proto_callback (u8 *buf, u8 size)
speed_aux[0].max = buf[4];
aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0);
break;
- case c ('B', 1):
- /* Find the zero position of the aux0.
- * - b: speed. */
- aux_traj_find_limit_start (&aux[0], buf[2], 0);
- break;
case c ('c', 3):
/* Move the aux1.
* - w: new position.
@@ -134,10 +129,23 @@ twi_proto_callback (u8 *buf, u8 size)
speed_aux[1].max = buf[4];
aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0);
break;
- case c ('C', 1):
- /* Find the zero position of the aux1.
- * - b: speed. */
- aux_traj_find_limit_start (&aux[1], buf[2], 0);
+ case c ('B', 5):
+ /* Find the zero position.
+ * - b: aux index.
+ * - b: speed.
+ * - b: use switch.
+ * - w: reset position. */
+ if (buf[2] < AC_ASSERV_AUX_NB)
+ {
+ if (buf[4])
+ aux_traj_find_zero_start (&aux[buf[2]], buf[3],
+ v8_to_v16 (buf[5], buf[6]), 0);
+ else
+ aux_traj_find_limit_start (&aux[buf[2]], buf[3],
+ v8_to_v16 (buf[5], buf[6]), 0);
+ }
+ else
+ buf[0] = 0;
break;
case c ('l', 4):
/* Clamp.