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 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) (limited to 'digital/mimot/src/dirty/aux.c') 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); -- cgit v1.2.3