Platform specific code for a robot using stepper motors.
This commit is contained in:
ksb
parent
13af443115
commit
20eaa31eff
17 changed files with 2262 additions and 0 deletions
648
platform/stepper-robot/stepper/stepper-interrupt.c
Normal file
648
platform/stepper-robot/stepper/stepper-interrupt.c
Normal file
|
|
@ -0,0 +1,648 @@
|
|||
#include <stepper-interrupt.h>
|
||||
#include <interrupt-utils.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
/* Timer frequency */
|
||||
#define TIMER_FREQ 748800
|
||||
|
||||
static StepperContext stepper_context;
|
||||
|
||||
static StepperAccSeq *free_seq = NULL;
|
||||
|
||||
StepperAccSeq *
|
||||
stepper_allocate_seq()
|
||||
{
|
||||
StepperAccSeq *seq;
|
||||
if (!free_seq) return NULL;
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
seq = free_seq;
|
||||
free_seq = seq->next;
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
return seq;
|
||||
}
|
||||
|
||||
void
|
||||
stepper_free_seq(StepperAccSeq *seq)
|
||||
{
|
||||
StepperAccSeq *s;
|
||||
if (!seq) return;
|
||||
s = seq;
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
while(s->next) s = s->next;
|
||||
s->next = free_seq;
|
||||
free_seq = seq;
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
}
|
||||
|
||||
static void
|
||||
do_step(StepperTimerStep *step)
|
||||
{
|
||||
const uint32_t *io_steps;
|
||||
StepperState *state = step->state;
|
||||
|
||||
|
||||
if (step->power >= STEPPER_POWER_ACC) {
|
||||
io_steps = state->acc_steps;
|
||||
} else if (step->power >= STEPPER_POWER_RUN) {
|
||||
io_steps = state->run_steps;
|
||||
} else {
|
||||
io_steps = state->hold_steps;
|
||||
}
|
||||
if (io_steps) {
|
||||
if (step->direction == STEPPER_DIRECTION_FORWARD){
|
||||
state->step_count++;
|
||||
/* dbg_putchar('+'); */
|
||||
if (++state->current_step == state->sequence_length)
|
||||
state->current_step = 0;
|
||||
} else {
|
||||
state->step_count--;
|
||||
/* dbg_putchar('-'); */
|
||||
if (state->current_step-- == 0)
|
||||
state->current_step = state->sequence_length-1;
|
||||
}
|
||||
*AT91C_PIOA_ODSR = (*AT91C_PIOA_ODSR & ~state->io_mask)
|
||||
| (io_steps[state->current_step] & state->io_mask);
|
||||
#ifdef TIMING_ERRORS
|
||||
{
|
||||
long err = ((long)stepper_context.timer_channel->TC_CV - (long)step->time);
|
||||
if (err >= (TIMER_FREQ/PPS/2)) {
|
||||
err -= TIMER_FREQ/PPS;
|
||||
} else if (err < -(TIMER_FREQ/PPS/2)) {
|
||||
err += TIMER_FREQ/PPS;
|
||||
}
|
||||
if (err < state->err_min) state->err_min = err;
|
||||
if (err > state->err_max) state->err_max = err;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
set_hold(StepperState *state) {
|
||||
*AT91C_PIOA_ODSR = (*AT91C_PIOA_ODSR & ~state->io_mask)
|
||||
| (state->hold_steps[state->current_step] & state->io_mask);
|
||||
}
|
||||
static void
|
||||
advance_step()
|
||||
{
|
||||
StepperTimerStep *current =stepper_context.current_step;
|
||||
AT91PS_TC timer = stepper_context.timer_channel;
|
||||
unsigned int now = timer->TC_CV;
|
||||
while (current && current->time <= now) {
|
||||
do_step(current);
|
||||
current = current->next;
|
||||
if (!current) break;
|
||||
timer->TC_RA = current->time;
|
||||
now = timer->TC_CV;
|
||||
}
|
||||
stepper_context.current_step = current;
|
||||
}
|
||||
|
||||
|
||||
static inline int64_t
|
||||
mulsu48_16(int64_t a, uint32_t b)
|
||||
{
|
||||
return a*(int64_t)b;
|
||||
}
|
||||
|
||||
/* Find a solution for s = a*t*t +v * t in the interval [t_low, t_high[ */
|
||||
static unsigned long
|
||||
solve_dist(long long s, long a, long long v, unsigned long t_low, unsigned long t_high)
|
||||
{
|
||||
long long s_low = mulsu48_16((a*(long)t_low+ v), t_low);
|
||||
long long s_high = mulsu48_16((a*(long)t_high + v), t_high);
|
||||
if (s >= s_low && s <= s_high) {
|
||||
while(t_low + 2 < t_high) {
|
||||
unsigned long t = (t_high + t_low) / 2;
|
||||
long long s_mid = mulsu48_16((a*(long)t + v), t);
|
||||
if (s < s_mid) {
|
||||
t_high = t;
|
||||
s_high = s_mid;
|
||||
} else {
|
||||
t_low = t;
|
||||
s_low = s_mid;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
while(t_low + 1 < t_high) {
|
||||
unsigned long t = (t_high + t_low) / 2;
|
||||
long long s_mid = mulsu48_16((a*(long)t + v), t);
|
||||
if (s > s_mid) {
|
||||
t_high = t;
|
||||
s_high = s_mid;
|
||||
} else {
|
||||
t_low = t;
|
||||
s_low = s_mid;
|
||||
}
|
||||
}
|
||||
}
|
||||
return (t_high + t_low) / 2;
|
||||
}
|
||||
|
||||
|
||||
#define HEAP_SIZE 65
|
||||
static StepperTimerStep step_heap[2][HEAP_SIZE];
|
||||
static unsigned short heap_pos = 0; /* Next free position in heap */
|
||||
static unsigned char current_heap = 0;
|
||||
|
||||
static StepperTimerStep *
|
||||
allocate_step()
|
||||
{
|
||||
if (heap_pos >= HEAP_SIZE) return NULL;
|
||||
return &step_heap[current_heap][heap_pos++];
|
||||
}
|
||||
|
||||
static void
|
||||
switch_step_heap()
|
||||
{
|
||||
current_heap ^= 1;
|
||||
heap_pos = 0;
|
||||
}
|
||||
|
||||
StepperTimerStep **
|
||||
insert_step(StepperTimerStep **at, StepperState *state,
|
||||
unsigned int time, uint8_t direction, uint8_t power)
|
||||
{
|
||||
StepperTimerStep *new_step;
|
||||
while(*at && (*at)->time <= time) {
|
||||
at = &(*at)->next;
|
||||
}
|
||||
new_step = allocate_step();
|
||||
if (!new_step) return at;
|
||||
new_step->next = *at;
|
||||
new_step->state = state;
|
||||
new_step->time = time;
|
||||
new_step->direction = direction;
|
||||
new_step->power = power;
|
||||
*at = new_step;
|
||||
/* dbg_putchar('!'); */
|
||||
return &new_step->next;
|
||||
}
|
||||
|
||||
/* Determine suitable power for the current state */
|
||||
static uint8_t
|
||||
get_power(StepperState *state)
|
||||
{
|
||||
if (state->acceleration != 0) return STEPPER_POWER_ACC;
|
||||
if (state->velocity == 0) return STEPPER_POWER_HOLD;
|
||||
return STEPPER_POWER_RUN;
|
||||
}
|
||||
|
||||
#define SQ(x) ((x)*(x))
|
||||
#define S_SCALING ((2LL*SQ((long long)TIMER_FREQ)) / DIST_SCALE )
|
||||
#define V_SCALING (2LL*TIMER_FREQ/VEL_SCALE)
|
||||
|
||||
|
||||
static void
|
||||
step_interval(StepperState *state)
|
||||
{
|
||||
unsigned int i;
|
||||
long long v = state->velocity * V_SCALING;
|
||||
long long a = state->acceleration;
|
||||
unsigned long t = 0;
|
||||
StepperTimerStep **at = &stepper_context.steps;
|
||||
if (state->n_steps >= 0) {
|
||||
long long s = -state->step_frac * S_SCALING;
|
||||
for (i = 0; i < state->n_steps; i++) {
|
||||
s+= DIST_SCALE * S_SCALING;
|
||||
t = solve_dist(s, a, v, t, TIMER_FREQ/PPS);
|
||||
/* printf("F%ld\n", t); */
|
||||
at = insert_step(at, state, t, STEPPER_DIRECTION_FORWARD, get_power(state));
|
||||
}
|
||||
} else {
|
||||
long long s = (DIST_SCALE - state->step_frac) * S_SCALING;
|
||||
for (i = 0; i < -state->n_steps; i++) {
|
||||
s-= DIST_SCALE * S_SCALING;
|
||||
t = solve_dist(s, a, v, t, TIMER_FREQ/PPS);
|
||||
/* printf("B%ld\n", t); */
|
||||
at = insert_step(at, state, t, STEPPER_DIRECTION_BACKWARD, get_power(state));
|
||||
}
|
||||
}
|
||||
}
|
||||
static void
|
||||
setup_speed(StepperState *state)
|
||||
{
|
||||
long steps;
|
||||
long step_frac;
|
||||
/* printf("%ld v= %ld s=%ld\n",stepper_context.period_count, */
|
||||
/* state->velocity, state->step_frac); */
|
||||
step_frac = (state->acceleration + 2 * state->velocity
|
||||
+ state->step_frac);
|
||||
steps = step_frac / DIST_SCALE;
|
||||
step_frac -= steps * DIST_SCALE;
|
||||
if (step_frac <0) {
|
||||
step_frac += DIST_SCALE;
|
||||
steps--;
|
||||
}
|
||||
|
||||
/* printf("step_frac=%ld (%f) steps=%ld\n",step_frac, */
|
||||
/* (double)step_frac/(double)(DIST_SCALE), steps); */
|
||||
state->n_steps = steps;
|
||||
step_interval(state);
|
||||
state->velocity += state->acceleration;
|
||||
state->step_frac = step_frac;
|
||||
state->step_full += steps;
|
||||
}
|
||||
|
||||
static void
|
||||
advance_period()
|
||||
{
|
||||
unsigned int s;
|
||||
StepperTimerStep *current =stepper_context.current_step;
|
||||
/* Do any remaining step */
|
||||
while (current) {
|
||||
do_step(current);
|
||||
current = current->next;
|
||||
}
|
||||
/* Start from the beginning */
|
||||
stepper_context.current_step = stepper_context.steps;
|
||||
stepper_context.steps = NULL;
|
||||
if (stepper_context.current_step) {
|
||||
stepper_context.timer_channel->TC_RA = stepper_context.current_step->time;
|
||||
} else {
|
||||
stepper_context.timer_channel->TC_RA = 0xffff;
|
||||
}
|
||||
/* In case there is a step very early in the period */
|
||||
advance_step();
|
||||
stepper_context.period_count++;
|
||||
*AT91C_AIC_EOICR = 0;
|
||||
for(s = 0; s < NUM_STEPPERS; s++) {
|
||||
StepperState *state = &stepper_context.steppers[s];
|
||||
StepperAccSeq *acc_seq;
|
||||
if (state->acceleration == 0 && state->velocity == 0) {
|
||||
/* Set hold power if stationary */
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
set_hold(state);
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
}
|
||||
while ((acc_seq = state->acceleration_sequence)
|
||||
&& acc_seq->period == stepper_context.period_count + 1) {
|
||||
state->acceleration_sequence = acc_seq->next;
|
||||
if (acc_seq->acceleration == STEPPER_ACC_INVALID) {
|
||||
if (stepper_context.user_callback) {
|
||||
stepper_context.user_callback(s, stepper_context.period_count);
|
||||
}
|
||||
} else {
|
||||
state->acceleration = acc_seq->acceleration;
|
||||
}
|
||||
acc_seq->next = NULL; /* Only free this one */
|
||||
stepper_free_seq(acc_seq);
|
||||
}
|
||||
setup_speed(&stepper_context.steppers[s]);
|
||||
}
|
||||
/* Prepare heap for next period */
|
||||
switch_step_heap();
|
||||
}
|
||||
|
||||
/* Here we have a proper stack frame and can use local variables */
|
||||
static void stepper_int_safe() __attribute((noinline));
|
||||
static void
|
||||
stepper_int_safe()
|
||||
{
|
||||
unsigned int status;
|
||||
status = stepper_context.timer_channel->TC_SR;
|
||||
if (status & AT91C_TC_CPAS) {
|
||||
advance_step();
|
||||
/* dbg_putchar('*'); */
|
||||
}
|
||||
if (status & AT91C_TC_CPCS) {
|
||||
advance_period();
|
||||
} else {
|
||||
*AT91C_AIC_EOICR = 0; /* End of Interrupt */
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void NACKEDFUNC stepper_int (void) {
|
||||
ISR_STORE();
|
||||
ISR_ENABLE_NEST();
|
||||
stepper_int_safe();
|
||||
ISR_DISABLE_NEST();
|
||||
ISR_RESTORE();
|
||||
}
|
||||
|
||||
static void
|
||||
stepper_state_init(StepperState *stepper)
|
||||
{
|
||||
stepper->step_count = 0;
|
||||
stepper->io_mask = 0;
|
||||
stepper->acc_steps = NULL;
|
||||
stepper->run_steps = NULL;
|
||||
stepper->hold_steps = NULL;
|
||||
stepper->current_step = 0;
|
||||
stepper->sequence_length = 0;
|
||||
|
||||
stepper->velocity = 0;
|
||||
stepper->acceleration = 0;
|
||||
stepper->step_full = 0;
|
||||
stepper->step_frac = 0;
|
||||
stepper->n_steps = 0;
|
||||
|
||||
#ifdef TIMING_ERRORS
|
||||
stepper->err_min = TIMER_FREQ;
|
||||
stepper->err_max = -TIMER_FREQ;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
stepper_init(AT91PS_TC timer, unsigned int id)
|
||||
{
|
||||
unsigned int s;
|
||||
stepper_context.flags = 0;
|
||||
stepper_context.timer_channel = timer;
|
||||
stepper_context.steps = NULL;
|
||||
stepper_context.current_step = NULL;
|
||||
stepper_context.period_count = 0;
|
||||
stepper_context.user_callback = NULL;
|
||||
|
||||
for (s = 0; s < NUM_STEPPERS; s++) {
|
||||
stepper_state_init(&stepper_context.steppers[s]);
|
||||
}
|
||||
timer->TC_CMR = (AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO
|
||||
| AT91C_TC_CLKS_TIMER_DIV3_CLOCK);
|
||||
timer->TC_RC = TIMER_FREQ / PPS;
|
||||
timer->TC_RA = 0xffff;
|
||||
timer->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
*AT91C_PMC_PCER = (1 << id);
|
||||
|
||||
AT91C_AIC_SMR[id] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 7;
|
||||
AT91C_AIC_SVR[id] = (unsigned long)stepper_int;
|
||||
*AT91C_AIC_IECR = (1 << id);
|
||||
timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
|
||||
}
|
||||
|
||||
void
|
||||
stepper_init_io(unsigned int stepper_index, uint32_t mask,
|
||||
const uint32_t *acc, const uint32_t *run,
|
||||
const uint32_t *hold, unsigned int nsteps)
|
||||
{
|
||||
StepperState *state;
|
||||
if (stepper_index >= NUM_STEPPERS) return;
|
||||
state = &stepper_context.steppers[stepper_index];
|
||||
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
|
||||
state->io_mask = mask;
|
||||
state->acc_steps = acc;
|
||||
state->run_steps = run;
|
||||
state->hold_steps = hold;
|
||||
state->current_step = 0;
|
||||
state->sequence_length = nsteps;
|
||||
*AT91C_PIOA_OWER = mask;
|
||||
*AT91C_PIOA_MDDR = mask;
|
||||
|
||||
*AT91C_PIOA_ODSR = ((*AT91C_PIOA_ODSR & ~mask)
|
||||
| (state->hold_steps[0] & mask));
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
*AT91C_PIOA_OER = mask;
|
||||
}
|
||||
|
||||
/**
|
||||
Append an acceleration sequence
|
||||
|
||||
Truncates the current acceleration sequence at the insertion time
|
||||
and appends the new sequence at that position.. The insertion time
|
||||
is the time of the first element of the new sequence. The
|
||||
truncation takes place after any elements with the acceleration set
|
||||
to STEPPER_ACC_INVALID (user callbacks) that has the same time as
|
||||
the insertion time. All other elements with the same time is
|
||||
replaced.
|
||||
|
||||
\param stepper_index Index of the stepper the sequence is intended for.
|
||||
\param new_seq A linked list of sequence elements to append.
|
||||
*/
|
||||
StepperResult
|
||||
stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq)
|
||||
{
|
||||
StepperResult res = STEPPER_ERR_TOO_LATE;
|
||||
StepperAccSeq **seqp;
|
||||
StepperState *state;
|
||||
if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX;
|
||||
state = &stepper_context.steppers[stepper_index];
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
seqp = &state->acceleration_sequence;
|
||||
while(*seqp && ((*seqp)->period < new_seq->period || ((*seqp)->period == new_seq->period && (*seqp)->acceleration == STEPPER_ACC_INVALID))) {
|
||||
seqp = &(*seqp)->next;
|
||||
}
|
||||
if (new_seq->period > stepper_context.period_count + 1) {
|
||||
/* Replace tail of sequence */
|
||||
if (*seqp) stepper_free_seq(*seqp);
|
||||
*seqp = new_seq;
|
||||
res = STEPPER_OK;
|
||||
}
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
Insert a callback mark
|
||||
|
||||
Inserts a callback mark at the indicated period. This will cause
|
||||
the callback procedure to be called just before that period,
|
||||
usually near the beginning of the previous period. Does not
|
||||
truncate the current sequence.
|
||||
|
||||
\param stepper_index Index of the stepper the callbak is intended for.
|
||||
\param period When the callback should be invoked
|
||||
|
||||
\sa stepper_set_callback_proc
|
||||
*/
|
||||
|
||||
StepperResult
|
||||
stepper_insert_callback(unsigned int stepper_index, unsigned int period)
|
||||
{
|
||||
StepperResult res = STEPPER_ERR_TOO_LATE;
|
||||
StepperAccSeq **seqp;
|
||||
StepperState *state;
|
||||
if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX;
|
||||
state = &stepper_context.steppers[stepper_index];
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
seqp = &state->acceleration_sequence;
|
||||
while(*seqp && (*seqp)->period < period) {
|
||||
seqp = &(*seqp)->next;
|
||||
}
|
||||
if (period > stepper_context.period_count + 1) {
|
||||
StepperAccSeq *new_seq = stepper_allocate_seq();
|
||||
if (!new_seq) {
|
||||
res = STEPPER_ERR_MEM;
|
||||
} else {
|
||||
new_seq->next = *seqp;
|
||||
*seqp = new_seq;
|
||||
new_seq->period = period;
|
||||
new_seq->acceleration = STEPPER_ACC_INVALID;
|
||||
res = STEPPER_OK;
|
||||
}
|
||||
}
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
return res;
|
||||
}
|
||||
|
||||
StepperResult
|
||||
stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc)
|
||||
{
|
||||
StepperAccSeq *seq = stepper_allocate_seq();
|
||||
/* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */
|
||||
if (!seq) return STEPPER_ERR_MEM;
|
||||
seq->next = NULL;
|
||||
seq->period = period;
|
||||
seq->acceleration = acc;
|
||||
return stepper_add_acc_seq(stepper_index, seq);
|
||||
}
|
||||
|
||||
void
|
||||
stepper_set_callback_proc(StepperUserCallback callback)
|
||||
{
|
||||
stepper_context.user_callback = callback;
|
||||
}
|
||||
|
||||
unsigned long
|
||||
stepper_current_period()
|
||||
{
|
||||
return stepper_context.period_count;
|
||||
}
|
||||
|
||||
long
|
||||
stepper_current_step(unsigned int stepper_index)
|
||||
{
|
||||
StepperState *state = &stepper_context.steppers[stepper_index];
|
||||
return state->step_count;
|
||||
}
|
||||
|
||||
long long
|
||||
stepper_step_frac(unsigned int stepper_index)
|
||||
{
|
||||
long long s;
|
||||
StepperState *state = &stepper_context.steppers[stepper_index];
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
s = state->step_full * DIST_SCALE + state->step_frac;
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
return s;
|
||||
}
|
||||
|
||||
long
|
||||
stepper_current_velocity(unsigned int stepper_index)
|
||||
{
|
||||
StepperState *state = &stepper_context.steppers[stepper_index];
|
||||
return state->velocity;
|
||||
}
|
||||
|
||||
/* Calculate the speed at given current given the current acceleration
|
||||
sequence. */
|
||||
unsigned long
|
||||
stepper_velocity(unsigned int stepper_index, unsigned long period)
|
||||
{
|
||||
long a;
|
||||
long v;
|
||||
unsigned long t;
|
||||
StepperState *state;
|
||||
StepperAccSeq *seq;
|
||||
state = &stepper_context.steppers[stepper_index];
|
||||
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
seq = state->acceleration_sequence;
|
||||
a = state->acceleration;
|
||||
v = state->velocity;
|
||||
t = stepper_context.period_count + 1;
|
||||
|
||||
while(seq && seq->period < period) {
|
||||
v += a * (seq->period - t);
|
||||
t = seq->period;
|
||||
a = seq->acceleration;
|
||||
seq = seq->next;
|
||||
}
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
v += a * (period - t);
|
||||
return v;
|
||||
}
|
||||
|
||||
/**
|
||||
Calculate the speed and position at specified period given the
|
||||
current acceleration sequence.
|
||||
|
||||
\param stepper_index Index of the stepper the callbak is intended for.
|
||||
\param period The period to calculate for
|
||||
\param Speed return
|
||||
\param Position return. In fractional steps
|
||||
|
||||
*/
|
||||
StepperResult
|
||||
stepper_state_at(unsigned int stepper_index, unsigned long period,
|
||||
long *velocity, long long *position)
|
||||
{
|
||||
long a;
|
||||
long v;
|
||||
long long s;
|
||||
unsigned long t;
|
||||
long dt;
|
||||
StepperState *state;
|
||||
StepperAccSeq *seq;
|
||||
state = &stepper_context.steppers[stepper_index];
|
||||
|
||||
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
if (period < stepper_context.period_count + 2) {
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
return STEPPER_ERR_TOO_LATE;
|
||||
}
|
||||
seq = state->acceleration_sequence;
|
||||
a = state->acceleration;
|
||||
v = state->velocity;
|
||||
t = stepper_context.period_count + 1;
|
||||
s = state->step_full * (long long)DIST_SCALE + state->step_frac;
|
||||
while(seq && seq->period < period) {
|
||||
dt = seq->period - t;
|
||||
s += (a * (long long)dt + 2 * v) * dt;
|
||||
v += a * (seq->period - t);
|
||||
t = seq->period;
|
||||
a = seq->acceleration;
|
||||
seq = seq->next;
|
||||
}
|
||||
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
|
||||
dt = period - t;
|
||||
*position = s + (a * (long long)dt + (DIST_SCALE/VEL_SCALE) * v) * dt;
|
||||
*velocity = v + a * dt;
|
||||
|
||||
return STEPPER_OK;
|
||||
}
|
||||
|
||||
|
||||
StepperResult
|
||||
stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp,
|
||||
unsigned long max_acc, long final_speed)
|
||||
{
|
||||
long start_period = *periodp;
|
||||
long v = stepper_velocity(stepper_index, start_period);
|
||||
if (final_speed == v) {
|
||||
return stepper_add_acc(stepper_index, start_period, 0);
|
||||
} else {
|
||||
StepperResult res;
|
||||
long a = (final_speed > v) ? max_acc : -max_acc;
|
||||
long t = ((long)(final_speed - v)) / a;
|
||||
long diff = (final_speed - v) - t * a;
|
||||
if (t > 0) {
|
||||
res = stepper_add_acc(stepper_index, start_period, a);
|
||||
if (res != STEPPER_OK) return res;
|
||||
}
|
||||
if (diff) {
|
||||
res = stepper_add_acc(stepper_index, start_period+t, diff);
|
||||
if (res != STEPPER_OK) return res;
|
||||
t++;
|
||||
}
|
||||
*periodp = start_period+t;
|
||||
return stepper_add_acc(stepper_index, start_period+t, 0);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef TIMING_ERRORS
|
||||
void
|
||||
stepper_timing_errors(unsigned int stepper_index, long *min, long *max)
|
||||
{
|
||||
StepperState *state;
|
||||
state = &stepper_context.steppers[stepper_index];
|
||||
*min = state->err_min;
|
||||
*max = state->err_max;
|
||||
state->err_max = -TIMER_FREQ;
|
||||
state->err_min = TIMER_FREQ;
|
||||
}
|
||||
#endif
|
||||
159
platform/stepper-robot/stepper/stepper-interrupt.h
Normal file
159
platform/stepper-robot/stepper/stepper-interrupt.h
Normal file
|
|
@ -0,0 +1,159 @@
|
|||
#ifndef __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__
|
||||
#define __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__
|
||||
|
||||
#include <AT91SAM7S64.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
/* Define periods/second */
|
||||
#define PPS 128
|
||||
|
||||
/* Scaling factor for distance */
|
||||
#define DIST_SCALE (2 * PPS * PPS)
|
||||
|
||||
/* Scaling factor for velocity */
|
||||
#define VEL_SCALE PPS
|
||||
|
||||
typedef struct _StepperContext StepperContext;
|
||||
typedef struct _StepperState StepperState;
|
||||
typedef struct _StepperTimerStep StepperTimerStep;
|
||||
typedef struct _StepperAccSeq StepperAccSeq;
|
||||
|
||||
#define MAX_STEPS_PER_PERIOD 40
|
||||
#define NUM_STEPPERS 2
|
||||
|
||||
#define STEPPER_MAX_VELOCITY 4000
|
||||
#define STEPPER_MAX_ACCELRATION 4000
|
||||
|
||||
struct _StepperAccSeq
|
||||
{
|
||||
StepperAccSeq *next;
|
||||
unsigned long period;
|
||||
long acceleration;
|
||||
};
|
||||
|
||||
#define STEPPER_ACC_INVALID LONG_MAX
|
||||
|
||||
#define TIMING_ERRORS
|
||||
|
||||
struct _StepperState
|
||||
{
|
||||
long step_count;
|
||||
uint32_t io_mask;
|
||||
const uint32_t *acc_steps; /* Stepping sequence when accelerating */
|
||||
const uint32_t *run_steps; /* Stepping sequence when running */
|
||||
const uint32_t *hold_steps; /* Stepping sequence when stationary */
|
||||
uint8_t current_step; /* in stepping sequence */
|
||||
uint8_t sequence_length;
|
||||
|
||||
long velocity; /* steps/second * PPS */
|
||||
long acceleration; /* steps/second^2 */
|
||||
long step_full; /* steps, same as step_count at period boundaries */
|
||||
long step_frac; /* (steps * PPS^2 * 2) % steps * PPS^2 */
|
||||
|
||||
long n_steps; /* full steps to move during this period */
|
||||
|
||||
StepperAccSeq *acceleration_sequence;
|
||||
|
||||
#ifdef TIMING_ERRORS
|
||||
long err_max;
|
||||
long err_min;
|
||||
#endif
|
||||
};
|
||||
|
||||
#define STEPPER_POWER_ACC 30
|
||||
#define STEPPER_POWER_RUN 20
|
||||
#define STEPPER_POWER_HOLD 10
|
||||
#define STEPPER_POWER_OFF 0
|
||||
|
||||
#define STEPPER_DIRECTION_NONE 0
|
||||
#define STEPPER_DIRECTION_FORWARD 1
|
||||
#define STEPPER_DIRECTION_BACKWARD 2
|
||||
|
||||
struct _StepperTimerStep
|
||||
{
|
||||
StepperTimerStep *next;
|
||||
StepperState *state;
|
||||
uint16_t time;
|
||||
uint8_t direction;
|
||||
uint8_t power;
|
||||
};
|
||||
|
||||
typedef void (*StepperUserCallback)(unsigned int stepper_index,
|
||||
unsigned long period);
|
||||
|
||||
struct _StepperContext
|
||||
{
|
||||
unsigned int flags;
|
||||
unsigned long period_count;
|
||||
AT91PS_TC timer_channel;
|
||||
StepperState steppers[NUM_STEPPERS];
|
||||
StepperTimerStep *steps;
|
||||
StepperTimerStep *current_step;
|
||||
StepperUserCallback user_callback;
|
||||
};
|
||||
|
||||
typedef unsigned int StepperResult;
|
||||
#define STEPPER_OK 0
|
||||
#define STEPPER_ERR_MEM 1
|
||||
#define STEPPER_ERR_TOO_LATE 2
|
||||
#define STEPPER_ERR_INDEX 3
|
||||
|
||||
void
|
||||
stepper_init(AT91PS_TC timer, unsigned int id);
|
||||
|
||||
void
|
||||
stepper_init_io(unsigned int stepper_index, uint32_t mask,
|
||||
const uint32_t *acc, const uint32_t *run,
|
||||
const uint32_t *hold, unsigned int nsteps);
|
||||
|
||||
/* Returns true if the new sequence was actually added or false
|
||||
if the index is illegal or the first step in the sequence is too soon */
|
||||
|
||||
StepperResult
|
||||
stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq);
|
||||
|
||||
StepperResult
|
||||
stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc);
|
||||
|
||||
StepperResult
|
||||
stepper_insert_callback(unsigned int stepper_index, unsigned int period);
|
||||
|
||||
void
|
||||
stepper_set_callback_proc(StepperUserCallback callback);
|
||||
|
||||
unsigned long
|
||||
stepper_current_period();
|
||||
|
||||
long
|
||||
stepper_current_step(unsigned int stepper_index);
|
||||
|
||||
long long
|
||||
stepper_step_frac(unsigned int stepper_index);
|
||||
|
||||
long
|
||||
stepper_current_velocity(unsigned int stepper_index);
|
||||
|
||||
|
||||
unsigned long
|
||||
stepper_velocity(unsigned int stepper_index, unsigned long period);
|
||||
|
||||
StepperResult
|
||||
stepper_state_at(unsigned int stepper_index, unsigned long period,
|
||||
long *velocity, long long *position);
|
||||
|
||||
StepperResult
|
||||
stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp,
|
||||
unsigned long max_acc, long final_speed);
|
||||
|
||||
StepperAccSeq *
|
||||
stepper_allocate_seq();
|
||||
|
||||
void
|
||||
stepper_free_seq(StepperAccSeq *seq);
|
||||
|
||||
#ifdef TIMING_ERRORS
|
||||
void
|
||||
stepper_timing_errors(unsigned int stepper_index, long *min, long *max);
|
||||
#endif
|
||||
|
||||
#endif /* __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ */
|
||||
193
platform/stepper-robot/stepper/stepper-move.c
Normal file
193
platform/stepper-robot/stepper/stepper-move.c
Normal file
|
|
@ -0,0 +1,193 @@
|
|||
#include <stdio.h>
|
||||
#include <stepper-interrupt.h>
|
||||
#include <stepper-move.h>
|
||||
#include <limits.h>
|
||||
|
||||
#if 0
|
||||
#define PRINTF(...) printf(__VA_ARGS__)
|
||||
#else
|
||||
#define PRINTF(...) do {} while (0)
|
||||
#endif
|
||||
|
||||
static unsigned int
|
||||
isqrt(unsigned long x)
|
||||
{
|
||||
unsigned int r;
|
||||
unsigned int b2 = 0x40000000;
|
||||
unsigned int b = 0x8000;
|
||||
while(x < b2) {
|
||||
b2 >>= 2;
|
||||
b >>= 1;
|
||||
}
|
||||
if (b == 0) return 0;
|
||||
r = b;
|
||||
b >>= 1;
|
||||
while(b > 0) {
|
||||
r += b;
|
||||
unsigned int t = r*r;
|
||||
if (t > x) {
|
||||
r -= b;
|
||||
}
|
||||
b >>=1;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
#define ACC_FIRST_UP 0
|
||||
#define ACC_K1_UP 1
|
||||
#define ACC_LAST_UP 2
|
||||
#define ACC_TOP 3
|
||||
#define ACC_FIRST_DOWN 4
|
||||
#define ACC_K1_DOWN 5
|
||||
#define ACC_LAST_DOWN 6
|
||||
#define ACC_END 7
|
||||
|
||||
typedef struct _AccDiff AccDiff;
|
||||
struct _AccDiff
|
||||
{
|
||||
long diff;
|
||||
unsigned long pos;
|
||||
};
|
||||
|
||||
|
||||
static inline long
|
||||
base_acc(unsigned long t,unsigned long n, unsigned long l, unsigned long a_max)
|
||||
{
|
||||
long a;
|
||||
if (t >= n) {
|
||||
if (t >= n+l) {
|
||||
a = -a_max;
|
||||
} else {
|
||||
a = 0;
|
||||
}
|
||||
} else {
|
||||
a = a_max;
|
||||
}
|
||||
return a;
|
||||
}
|
||||
|
||||
static AccDiff acc[ACC_END+1];
|
||||
StepperResult
|
||||
stepper_move(unsigned int stepper_index, unsigned long *periodp,
|
||||
unsigned long a_max,unsigned long v_max, long s_end)
|
||||
{
|
||||
unsigned long start_period = *periodp;
|
||||
unsigned long s;
|
||||
unsigned long ds;
|
||||
unsigned long l;
|
||||
unsigned long da0;
|
||||
unsigned long k1 = 0;
|
||||
unsigned long n = (v_max+a_max-1)/a_max;
|
||||
unsigned long a_speed_adj = v_max - (n-1)*a_max;
|
||||
unsigned long s_res;
|
||||
long d;
|
||||
if (s_end >= 0) {
|
||||
s_res = s_end/2;
|
||||
} else {
|
||||
s_res = (-s_end)/2;
|
||||
}
|
||||
d = s_res - (long)a_max*(n*n-1) - (long)a_speed_adj;
|
||||
|
||||
acc[ACC_END].diff = 0;
|
||||
acc[ACC_END].pos = UINT_MAX;
|
||||
if (d < 0) {
|
||||
l = 0;
|
||||
n = isqrt(s_res/a_max);
|
||||
if (n*(unsigned long long)n*a_max < s_res) n++;
|
||||
a_speed_adj = a_max;
|
||||
acc[ACC_LAST_UP].diff=0;
|
||||
acc[ACC_FIRST_DOWN].diff=0;
|
||||
} else {
|
||||
l = (d+v_max-1)/v_max;
|
||||
acc[ACC_LAST_UP].diff= a_speed_adj - a_max;
|
||||
acc[ACC_FIRST_DOWN].diff= a_max - a_speed_adj;
|
||||
}
|
||||
acc[ACC_LAST_UP].pos = n-1;
|
||||
acc[ACC_FIRST_DOWN].pos = n+l;
|
||||
|
||||
s = a_max*(n*n-1) + a_speed_adj + l * (a_max*(n-1) + a_speed_adj);
|
||||
ds = s-s_res;
|
||||
|
||||
da0 = ds/(2*n+l-1);
|
||||
acc[ACC_FIRST_UP].diff = -da0;
|
||||
acc[ACC_LAST_DOWN].diff = da0;
|
||||
acc[ACC_FIRST_UP].pos = 0;
|
||||
acc[ACC_LAST_DOWN].pos = 2*n+l-1;
|
||||
ds -= da0*(2*n+l-1);
|
||||
|
||||
acc[ACC_K1_UP].diff = 0;
|
||||
acc[ACC_K1_DOWN].diff = 0;
|
||||
acc[ACC_K1_UP].pos = 0;
|
||||
acc[ACC_K1_DOWN].pos = 2*n+l-1;
|
||||
|
||||
acc[ACC_TOP].diff = 0;
|
||||
acc[ACC_TOP].pos = n;
|
||||
|
||||
if (ds > 0) {
|
||||
k1 = (2*n+l -ds)/2;
|
||||
if (k1 < n) {
|
||||
|
||||
acc[ACC_K1_UP].diff = -1;
|
||||
acc[ACC_K1_DOWN].diff = 1;
|
||||
acc[ACC_K1_UP].pos = k1;
|
||||
acc[ACC_K1_DOWN].pos = 2*n+l-1 - k1;
|
||||
ds -= (2*(n-k1)+l-1);
|
||||
}
|
||||
if (ds > 0) {
|
||||
acc[ACC_LAST_UP].diff--;
|
||||
acc[ACC_TOP].diff = 1;
|
||||
acc[ACC_TOP].pos = n+ds-1;
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
{
|
||||
unsigned int k;
|
||||
PRINTF("n=%ld l=%ld a_max=%ld v_max=%ld s_res=%ld\n",
|
||||
n,l ,a_max, v_max, s_res);
|
||||
for (k = 0; k < 7; k++) {
|
||||
PRINTF(" %ld@%ld", acc[k].diff, acc[k].pos);
|
||||
}
|
||||
PRINTF("\n");
|
||||
}
|
||||
#endif
|
||||
{
|
||||
StepperResult res;
|
||||
unsigned int k;
|
||||
unsigned long t = 0;
|
||||
long da = 0;
|
||||
long a_prev = ULONG_MAX;
|
||||
for (k = 0; k < ACC_END; k++) {
|
||||
long a;
|
||||
da += acc[k].diff;
|
||||
if (acc[k].pos != acc[k+1].pos) { /* Next position is different */
|
||||
if (t != acc[k].pos) {
|
||||
a = base_acc(t,n,l,a_max);
|
||||
if (s_end < 0) a = -a;
|
||||
if (a_prev != a) {
|
||||
res = stepper_add_acc(stepper_index, t+start_period, a);
|
||||
if (res != STEPPER_OK) return res;
|
||||
PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period);
|
||||
a_prev = a;
|
||||
}
|
||||
t = acc[k].pos;
|
||||
}
|
||||
a = da + base_acc(t,n,l,a_max);
|
||||
if (s_end < 0) a = -a;
|
||||
if (a_prev != a) {
|
||||
res = stepper_add_acc(stepper_index, t+start_period, a);
|
||||
if (res != STEPPER_OK) return res;
|
||||
PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period);
|
||||
a_prev = a;
|
||||
}
|
||||
t++;
|
||||
da = 0;
|
||||
}
|
||||
}
|
||||
res = stepper_add_acc(stepper_index, t+start_period, 0);
|
||||
PRINTF("%d: %d@%ld\n", stepper_index, 0, t+start_period);
|
||||
if (res != STEPPER_OK) return res;
|
||||
*periodp += t;
|
||||
}
|
||||
return STEPPER_OK;
|
||||
}
|
||||
|
||||
8
platform/stepper-robot/stepper/stepper-move.h
Normal file
8
platform/stepper-robot/stepper/stepper-move.h
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
#ifndef __STEPPER_MOVE_H__9UFUHMYMYS__
|
||||
#define __STEPPER_MOVE_H__9UFUHMYMYS__
|
||||
|
||||
StepperResult
|
||||
stepper_move(unsigned int stepper_index, unsigned long *periodp,
|
||||
unsigned long a_max,unsigned long v_max, long s_end);
|
||||
|
||||
#endif /* __STEPPER_MOVE_H__9UFUHMYMYS__ */
|
||||
100
platform/stepper-robot/stepper/stepper-steps.h
Normal file
100
platform/stepper-robot/stepper/stepper-steps.h
Normal file
|
|
@ -0,0 +1,100 @@
|
|||
#include <AT91SAM7S64.h>
|
||||
|
||||
#ifndef MAX
|
||||
#define MIN(a,b) (((a) > (b)) ? (b) : (a))
|
||||
#endif
|
||||
|
||||
/* Pins for stepper 0 */
|
||||
#define STEPPER0_I00 AT91C_PIO_PA17
|
||||
#define STEPPER0_I01 AT91C_PIO_PA18
|
||||
#define STEPPER0_PHASE0 AT91C_PIO_PA19
|
||||
|
||||
#define STEPPER0_I10 AT91C_PIO_PA21
|
||||
#define STEPPER0_I11 AT91C_PIO_PA22
|
||||
#define STEPPER0_PHASE1 AT91C_PIO_PA23
|
||||
|
||||
/* Pins for stepper 1 */
|
||||
#define STEPPER1_I00 AT91C_PIO_PA24
|
||||
#define STEPPER1_I01 AT91C_PIO_PA25
|
||||
#define STEPPER1_PHASE0 AT91C_PIO_PA26
|
||||
|
||||
#define STEPPER1_I10 AT91C_PIO_PA27
|
||||
#define STEPPER1_I11 AT91C_PIO_PA28
|
||||
#define STEPPER1_PHASE1 AT91C_PIO_PA29
|
||||
|
||||
/* Common pins */
|
||||
#define STEPPER_INHIBIT AT91C_PIO_PA7
|
||||
|
||||
|
||||
#define STEPPER_IOMASK(s) (STEPPER##s##_I00 | STEPPER##s##_I01 | STEPPER##s##_PHASE0 \
|
||||
| STEPPER##s##_I10 | STEPPER##s##_I11 | STEPPER##s##_PHASE1)
|
||||
#define STEPPER_PHASE(s,p,l) ((((l) < 2) ? (STEPPER##s##_I##p##1) : 0) | (((l) & 1) ? 0 : (STEPPER##s##_I##p##0)))
|
||||
|
||||
#define STEPPER_STEP(s, l0, l1) (\
|
||||
((l0 >= 0) ? STEPPER_PHASE(s,0, l0):(STEPPER_PHASE(s,0, -(l0))|STEPPER##s##_PHASE0)) | \
|
||||
((l1 >= 0) ? STEPPER_PHASE(s,1, l1):(STEPPER_PHASE(s,1,-(l1))|STEPPER##s##_PHASE1)))
|
||||
|
||||
|
||||
#define FULL_STEP_BOTH(s,l) {\
|
||||
STEPPER_STEP(s,(l),(l)), \
|
||||
STEPPER_STEP(s,(l),-(l)), \
|
||||
STEPPER_STEP(s,-(l),-(l)), \
|
||||
STEPPER_STEP(s,-(l),(l))}
|
||||
|
||||
|
||||
#define FULL_STEP_SINGLE(s,l) {\
|
||||
STEPPER_STEP(s,0,(l)), \
|
||||
STEPPER_STEP(s,(l),0), \
|
||||
STEPPER_STEP(s,0,-(l)), \
|
||||
STEPPER_STEP(s,-(l),0)}
|
||||
|
||||
#define HALF_STEP(s,l) {\
|
||||
STEPPER_STEP(s,0,(l)), \
|
||||
STEPPER_STEP(s,(l),(l)), \
|
||||
STEPPER_STEP(s,(l),0), \
|
||||
STEPPER_STEP(s,(l),-(l)), \
|
||||
STEPPER_STEP(s,0,-(l)), \
|
||||
STEPPER_STEP(s,-(l),-(l)), \
|
||||
STEPPER_STEP(s,-(l),0), \
|
||||
STEPPER_STEP(s,-(l),(l))}
|
||||
|
||||
|
||||
#define MINI_STEP(s,l) {\
|
||||
STEPPER_STEP(s,0,(l)), \
|
||||
STEPPER_STEP(s,1,MIN((l),2)), \
|
||||
STEPPER_STEP(s,MIN((l),2),1), \
|
||||
STEPPER_STEP(s,(l),0), \
|
||||
STEPPER_STEP(s,MIN((l),2),-1), \
|
||||
STEPPER_STEP(s,1,-MIN((l),2)), \
|
||||
STEPPER_STEP(s,0,-(l)), \
|
||||
STEPPER_STEP(s,-1,-MIN((l),2)), \
|
||||
STEPPER_STEP(s,-MIN((l),2),-1), \
|
||||
STEPPER_STEP(s,-(l),0), \
|
||||
STEPPER_STEP(s,-MIN((l),2),1), \
|
||||
STEPPER_STEP(s,-1,MIN((l),2))}
|
||||
|
||||
#define MICRO_STEP(s,l) {\
|
||||
STEPPER_STEP(s,0,(l)), \
|
||||
STEPPER_STEP(s,1,(l)), \
|
||||
STEPPER_STEP(s,MIN((l),2),(l)), \
|
||||
STEPPER_STEP(s,(l),(l)), \
|
||||
STEPPER_STEP(s,(l),MIN((l),2)), \
|
||||
STEPPER_STEP(s,(l),1), \
|
||||
STEPPER_STEP(s,(l),0), \
|
||||
STEPPER_STEP(s,(l),-1), \
|
||||
STEPPER_STEP(s,(l),-MIN((l),2)), \
|
||||
STEPPER_STEP(s,(l),-(l)), \
|
||||
STEPPER_STEP(s,MIN((l),2),-(l)), \
|
||||
STEPPER_STEP(s,1,-(l)), \
|
||||
STEPPER_STEP(s,0,-(l)), \
|
||||
STEPPER_STEP(s,-1,-(l)), \
|
||||
STEPPER_STEP(s,-MIN((l),2),-(l)), \
|
||||
STEPPER_STEP(s,-(l),-(l)), \
|
||||
STEPPER_STEP(s,-(l),-MIN((l),2)), \
|
||||
STEPPER_STEP(s,-(l),-1), \
|
||||
STEPPER_STEP(s,-(l),0), \
|
||||
STEPPER_STEP(s,-(l),1), \
|
||||
STEPPER_STEP(s,-(l),MIN((l),2)), \
|
||||
STEPPER_STEP(s,-(l),(l)), \
|
||||
STEPPER_STEP(s,-MIN((l),2),(l)), \
|
||||
STEPPER_STEP(s,-1,(l))}
|
||||
Loading…
Add table
Add a link
Reference in a new issue