klipper/src/stepper.c
Kevin O'Connor 6220cdda92 stepper: Support rescheduling of step events on faster MCUs
On faster MCUs the step and unstep events may be too close for the
stepper motor driver.  Add a CONFIG_NO_UNSTEP_DELAY build option and
support the case where it is not set.  This allows faster MCUs to
schedule two events for each step (one for the step and one for the
unstep).

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
2016-06-14 14:24:14 -04:00

253 lines
7.3 KiB
C

// Handling of stepper drivers.
//
// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <stddef.h> // NULL
#include "autoconf.h" // CONFIG_*
#include "basecmd.h" // alloc_oid
#include "board/gpio.h" // gpio_out_write
#include "board/irq.h" // irq_disable
#include "command.h" // DECL_COMMAND
#include "sched.h" // struct timer
#include "stepper.h" // command_config_stepper
/****************************************************************
* Steppers
****************************************************************/
struct stepper {
struct timer time;
uint32_t interval;
int16_t add;
#if CONFIG_NO_UNSTEP_DELAY
uint16_t count;
#define next_step_time time.waketime
#else
uint32_t count;
uint32_t next_step_time;
#endif
struct gpio_out step_pin, dir_pin;
uint32_t position;
struct move *first, **plast;
uint32_t min_stop_interval;
// gcc (pre v6) does better optimization when uint8_t are bitfields
uint8_t flags : 8;
};
enum { MF_DIR=1 };
enum { SF_LAST_DIR=1, SF_NEXT_DIR=2, SF_INVERT_STEP=4, SF_HAVE_ADD=8 };
// Setup a stepper for the next move in its queue
static uint_fast8_t
stepper_load_next(struct stepper *s, uint32_t min_next_time)
{
struct move *m = s->first;
if (!m) {
if (s->interval - s->add < s->min_stop_interval)
shutdown("No next step");
s->count = 0;
return SF_DONE;
}
s->next_step_time += m->interval;
s->add = m->add;
s->interval = m->interval + m->add;
if (CONFIG_NO_UNSTEP_DELAY) {
// On slow mcus see if the add can be optimized away
s->flags = m->add ? s->flags | SF_HAVE_ADD : s->flags & ~SF_HAVE_ADD;
s->count = m->count;
} else {
// On faster mcus, it is necessary to schedule unstep events
// and so there are twice as many events. Also check that the
// next step event isn't too close to the last unstep.
if (unlikely(sched_is_before(s->next_step_time, min_next_time))) {
if ((int32_t)(s->next_step_time - min_next_time)
< (int32_t)(-sched_from_us(1000)))
shutdown("stepper too far in past");
s->time.waketime = min_next_time;
} else {
s->time.waketime = s->next_step_time;
}
s->count = m->count * 2;
}
if (m->flags & MF_DIR) {
s->position = -s->position + m->count;
gpio_out_toggle(s->dir_pin);
} else {
s->position += m->count;
}
s->first = m->next;
move_free(m);
return SF_RESCHEDULE;
}
#define UNSTEP_TIME sched_from_us(1)
// Timer callback - step the given stepper.
uint_fast8_t
stepper_event(struct timer *t)
{
struct stepper *s = container_of(t, struct stepper, time);
if (CONFIG_NO_UNSTEP_DELAY) {
// On slower mcus it is possible to simply step and unstep in
// the same timer event.
gpio_out_toggle(s->step_pin);
uint16_t count = s->count - 1;
if (likely(count)) {
s->count = count;
s->time.waketime += s->interval;
gpio_out_toggle(s->step_pin);
if (s->flags & SF_HAVE_ADD)
s->interval += s->add;
return SF_RESCHEDULE;
}
uint_fast8_t ret = stepper_load_next(s, 0);
gpio_out_toggle(s->step_pin);
return ret;
}
// On faster mcus, it is necessary to schedule the unstep event
uint32_t min_next_time = sched_read_time() + UNSTEP_TIME;
gpio_out_toggle(s->step_pin);
s->count--;
if (likely(s->count & 1))
// Schedule unstep event
goto reschedule_min;
if (likely(s->count)) {
s->next_step_time += s->interval;
s->interval += s->add;
if (unlikely(sched_is_before(s->next_step_time, min_next_time)))
// The next step event is too close - push it back
goto reschedule_min;
s->time.waketime = s->next_step_time;
return SF_RESCHEDULE;
}
return stepper_load_next(s, min_next_time);
reschedule_min:
s->time.waketime = min_next_time;
return SF_RESCHEDULE;
}
void
command_config_stepper(uint32_t *args)
{
struct stepper *s = alloc_oid(args[0], command_config_stepper, sizeof(*s));
if (!CONFIG_INLINE_STEPPER_HACK)
s->time.func = stepper_event;
s->flags = args[4] ? SF_INVERT_STEP : 0;
s->step_pin = gpio_out_setup(args[1], s->flags & SF_INVERT_STEP ? 1 : 0);
s->dir_pin = gpio_out_setup(args[2], 0);
s->min_stop_interval = args[3];
s->position = STEPPER_POSITION_BIAS;
}
DECL_COMMAND(command_config_stepper,
"config_stepper oid=%c step_pin=%c dir_pin=%c"
" min_stop_interval=%u invert_step=%c");
// Schedule a set of steps with a given timing
void
command_queue_step(uint32_t *args)
{
struct stepper *s = lookup_oid(args[0], command_config_stepper);
struct move *m = move_alloc();
m->interval = args[1];
m->count = args[2];
if (!m->count)
shutdown("Invalid count parameter");
m->add = args[3];
m->next = NULL;
m->flags = 0;
irq_disable();
if (!!(s->flags & SF_LAST_DIR) != !!(s->flags & SF_NEXT_DIR)) {
s->flags ^= SF_LAST_DIR;
m->flags |= MF_DIR;
}
if (s->count) {
if (s->first)
*s->plast = m;
else
s->first = m;
s->plast = &m->next;
} else {
s->first = m;
stepper_load_next(s, s->next_step_time + m->interval);
sched_timer(&s->time);
}
irq_enable();
}
DECL_COMMAND(command_queue_step,
"queue_step oid=%c interval=%u count=%hu add=%hi");
// Set the direction of the next queued step
void
command_set_next_step_dir(uint32_t *args)
{
struct stepper *s = lookup_oid(args[0], command_config_stepper);
uint8_t nextdir = args[1] ? SF_NEXT_DIR : 0;
irq_disable();
s->flags = (s->flags & ~SF_NEXT_DIR) | nextdir;
irq_enable();
}
DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c");
// Set an absolute time that the next step will be relative to
void
command_reset_step_clock(uint32_t *args)
{
struct stepper *s = lookup_oid(args[0], command_config_stepper);
uint32_t waketime = args[1];
if (s->count)
shutdown("Can't reset time when stepper active");
s->next_step_time = waketime;
}
DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u");
// Return the current stepper position. Caller must disable irqs.
uint32_t
stepper_get_position(struct stepper *s)
{
uint32_t position = s->position;
if (CONFIG_NO_UNSTEP_DELAY)
position -= s->count;
else
position -= s->count / 2;
if (position & 0x80000000)
return -position;
return position;
}
// Stop all moves for a given stepper (used in end stop homing). IRQs
// must be off.
void
stepper_stop(struct stepper *s)
{
sched_del_timer(&s->time);
s->position = stepper_get_position(s);
s->count = 0;
s->flags &= SF_INVERT_STEP;
gpio_out_write(s->dir_pin, 0);
gpio_out_write(s->step_pin, s->flags & SF_INVERT_STEP ? 1 : 0);
while (s->first) {
struct move *next = s->first->next;
move_free(s->first);
s->first = next;
}
}
static void
stepper_shutdown(void)
{
uint8_t i;
struct stepper *s;
foreach_oid(i, s, command_config_stepper) {
s->first = NULL;
stepper_stop(s);
}
}
DECL_SHUTDOWN(stepper_shutdown);