mirror of
https://github.com/oliverschmidt/contiki.git
synced 2024-11-19 03:05:14 +00:00
650 lines
18 KiB
C
650 lines
18 KiB
C
#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 + 2;
|
|
|
|
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 + 2;
|
|
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);
|
|
/* printf("%ld @ %ld\n", v, 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
|