summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-10 08:38:26 +0200
committerNicolas Schodet2010-05-10 08:38:26 +0200
commit2781ad249062dac90dd5b86da6b2d54bfa597e7e (patch)
treeb54d857eb61fa1dafd16cb170468ac54ad425ebb
parent022d4aa4f9e6ee7aad29a46d4ce3df5383426f6b (diff)
digital/mimot: add clamp command
-rw-r--r--digital/mimot/src/dirty/aux.c34
-rw-r--r--digital/mimot/src/dirty/aux.h6
-rw-r--r--digital/mimot/src/dirty/main.c12
-rw-r--r--digital/mimot/src/dirty/twi_proto.c11
-rw-r--r--digital/mimot/tools/mimot/mimot.py7
5 files changed, 70 insertions, 0 deletions
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
index a5a65a8d..19268dd0 100644
--- a/digital/mimot/src/dirty/aux.c
+++ b/digital/mimot/src/dirty/aux.c
@@ -51,6 +51,8 @@ enum
AUX_TRAJ_GOTO,
/* Goto position, try to unblock. */
AUX_TRAJ_GOTO_UNBLOCK,
+ /* Speed control, apply open loop PWM on blocking. */
+ AUX_TRAJ_CLAMP,
/* Find zero mode, turn until zero is not seen. */
AUX_TRAJ_FIND_ZERO_NOT,
/* Find zero mode, turn until zero is seen. */
@@ -135,6 +137,35 @@ aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq)
aux->state->variant = 4;
}
+/** Speed control, then clamp. */
+void
+aux_traj_clamp (struct aux_t *aux)
+{
+ /* If blocking, stop control, clamp. */
+ if (aux->speed->pos->blocked_counter
+ > aux->speed->pos->blocked_counter_limit)
+ {
+ state_finish (aux->state);
+ pos_reset (aux->speed->pos);
+ aux->speed->cur = 0;
+ aux->state->mode = MODE_PWM;
+ pwm_set (aux->pwm, aux->clampin_pwm);
+ aux->traj_mode = AUX_TRAJ_DONE;
+ }
+}
+
+void
+aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm,
+ uint8_t seq)
+{
+ aux->traj_mode = AUX_TRAJ_CLAMP;
+ aux->clampin_pwm = clampin_pwm;
+ aux->speed->use_pos = 0;
+ aux->speed->cons = speed << 8;
+ state_start (aux->state, MODE_TRAJ, seq);
+ aux->state->variant = 4;
+}
+
/** Find zero mode. */
void
aux_traj_find_zero (struct aux_t *aux)
@@ -208,6 +239,9 @@ aux_traj_update_single (struct aux_t *aux)
case AUX_TRAJ_GOTO_UNBLOCK:
aux_traj_goto (aux);
break;
+ case AUX_TRAJ_CLAMP:
+ aux_traj_clamp (aux);
+ break;
case AUX_TRAJ_FIND_ZERO_NOT:
case AUX_TRAJ_FIND_ZERO:
aux_traj_find_zero (aux);
diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h
index b541f3eb..f31b789a 100644
--- a/digital/mimot/src/dirty/aux.h
+++ b/digital/mimot/src/dirty/aux.h
@@ -40,6 +40,8 @@ struct aux_t
uint8_t traj_mode;
/** Goto position position. */
uint32_t goto_pos;
+ /** Clamping PWM. */
+ int16_t clampin_pwm;
/** Wait counter. */
uint16_t wait;
/** Top zero port input register. */
@@ -62,6 +64,10 @@ void
aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq);
void
+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);
void
diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
index d826dd98..b3d93aa0 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -275,6 +275,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
break;
+ case c ('y', 5):
+ /* Auxiliary clamp.
+ * - b: aux index.
+ * - b: speed.
+ * - w: clamping PWM.
+ * - b: sequence number. */
+ if (!auxp) { proto_send0 ('?'); return; }
+ if (args[4] == state->sequence)
+ break;
+ aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]),
+ args[4]);
+ break;
case c ('y', 3):
/* Auxiliary find zero.
* - b: aux index.
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 37c1b64f..a6894109 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -139,6 +139,17 @@ twi_proto_callback (u8 *buf, u8 size)
* - b: speed. */
aux_traj_find_limit_start (&aux[1], buf[2], 0);
break;
+ case c ('l', 4):
+ /* Clamp.
+ * - b: aux index.
+ * - b: speed.
+ * - w: claming PWM. */
+ if (buf[2] < AC_ASSERV_AUX_NB)
+ aux_traj_clamp_start (&aux[buf[2]], buf[3],
+ v8_to_v16 (buf[4], buf[5]), 0);
+ else
+ buf[0] = 0;
+ break;
case c ('p', x):
/* Set parameters. */
if (twi_proto_params (&buf[2], size - 2) != 0)
diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py
index 2844c741..013a85ff 100644
--- a/digital/mimot/tools/mimot/mimot.py
+++ b/digital/mimot/tools/mimot/mimot.py
@@ -155,6 +155,13 @@ class Proto:
self.proto.send ('y', 'BhB', i, pos, self.aseq[i])
self.wait (self.finished, auto = True)
+ def clamp (self, w, s, pwm):
+ """Clamp (speed control, then open loop PWM)."""
+ i = self._index[w]
+ self.aseq[i] += 1
+ self.proto.send ('y', 'BBhB', i, s, pwm, self.aseq[i])
+ self.wait (self.finished, auto = True)
+
def send_param (self):
"""Send all parameters."""
p = self.param