mirror of
https://github.com/andreili/klipper.git
synced 2025-08-24 03:44:06 +02:00
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>
253 lines
7.3 KiB
C
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);
|