From 2781ad249062dac90dd5b86da6b2d54bfa597e7e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 10 May 2010 08:38:26 +0200 Subject: digital/mimot: add clamp command --- digital/mimot/src/dirty/aux.c | 34 ++++++++++++++++++++++++++++++++++ digital/mimot/src/dirty/aux.h | 6 ++++++ digital/mimot/src/dirty/main.c | 12 ++++++++++++ digital/mimot/src/dirty/twi_proto.c | 11 +++++++++++ digital/mimot/tools/mimot/mimot.py | 7 +++++++ 5 files changed, 70 insertions(+) (limited to 'digital') 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. */ @@ -61,6 +63,10 @@ aux_pos_update (void); 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); 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 -- cgit v1.2.3