summaryrefslogtreecommitdiff
path: root/digital/mimot/src/dirty/aux.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/mimot/src/dirty/aux.c')
-rw-r--r--digital/mimot/src/dirty/aux.c34
1 files changed, 34 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);