mirror of https://github.com/Desuuuu/klipper.git
stepper: Introduce stepper_get_position command and remove from endstop.c
Move the logic to calculate and report the stepper's current position from endstop.c to stepper.c. This localizes the stepper code into stepper.c. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
8d6ecd9af8
commit
a38437f378
|
@ -251,6 +251,12 @@ Stepper commands
|
||||||
so that the next step is relative to the supplied 'clock' time. The
|
so that the next step is relative to the supplied 'clock' time. The
|
||||||
host usually only sends this command at the start of a print.
|
host usually only sends this command at the start of a print.
|
||||||
|
|
||||||
|
* stepper_get_position oid=%c : This command causes the firmware to
|
||||||
|
generate a "stepper_position" response message with the stepper's
|
||||||
|
current position. The position is the total number of steps
|
||||||
|
generated with dir=1 minus the total number of steps generated with
|
||||||
|
dir=0.
|
||||||
|
|
||||||
* end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c : This
|
* end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c : This
|
||||||
command is used during stepper "homing" operations. To use this
|
command is used during stepper "homing" operations. To use this
|
||||||
command a 'config_end_stop' command with the same 'oid' parameter
|
command a 'config_end_stop' command with the same 'oid' parameter
|
||||||
|
|
|
@ -43,6 +43,8 @@ class MCU_stepper:
|
||||||
"set_next_step_dir oid=%c dir=%c")
|
"set_next_step_dir oid=%c dir=%c")
|
||||||
self._reset_cmd = mcu.lookup_command(
|
self._reset_cmd = mcu.lookup_command(
|
||||||
"reset_step_clock oid=%c clock=%u")
|
"reset_step_clock oid=%c clock=%u")
|
||||||
|
self._get_position_cmd = mcu.lookup_command(
|
||||||
|
"stepper_get_position oid=%c")
|
||||||
ffi_main, self.ffi_lib = chelper.get_ffi()
|
ffi_main, self.ffi_lib = chelper.get_ffi()
|
||||||
self._stepqueue = ffi_main.gc(self.ffi_lib.stepcompress_alloc(
|
self._stepqueue = ffi_main.gc(self.ffi_lib.stepcompress_alloc(
|
||||||
max_error, self._step_cmd.msgid
|
max_error, self._step_cmd.msgid
|
||||||
|
@ -51,13 +53,9 @@ class MCU_stepper:
|
||||||
self.print_to_mcu_time = mcu.print_to_mcu_time
|
self.print_to_mcu_time = mcu.print_to_mcu_time
|
||||||
def get_oid(self):
|
def get_oid(self):
|
||||||
return self._oid
|
return self._oid
|
||||||
def get_invert_dir(self):
|
|
||||||
return self._invert_dir
|
|
||||||
def set_position(self, pos):
|
def set_position(self, pos):
|
||||||
self._mcu_position_offset += self.commanded_position - pos
|
self._mcu_position_offset += self.commanded_position - pos
|
||||||
self.commanded_position = pos
|
self.commanded_position = pos
|
||||||
def set_mcu_position(self, pos):
|
|
||||||
self._mcu_position_offset = pos - self.commanded_position
|
|
||||||
def get_mcu_position(self):
|
def get_mcu_position(self):
|
||||||
return self.commanded_position + self._mcu_position_offset
|
return self.commanded_position + self._mcu_position_offset
|
||||||
def note_homing_start(self, homing_clock):
|
def note_homing_start(self, homing_clock):
|
||||||
|
@ -71,6 +69,14 @@ class MCU_stepper:
|
||||||
ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0)
|
ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0)
|
||||||
if ret:
|
if ret:
|
||||||
raise error("Internal error in stepcompress")
|
raise error("Internal error in stepcompress")
|
||||||
|
def note_homing_triggered(self):
|
||||||
|
params = self._mcu.serial.send_with_response(
|
||||||
|
self._get_position_cmd.encode(self._oid),
|
||||||
|
'stepper_position', self._oid)
|
||||||
|
pos = params['pos']
|
||||||
|
if self._invert_dir:
|
||||||
|
pos = -pos
|
||||||
|
self._mcu_position_offset = pos - self.commanded_position
|
||||||
def reset_step_clock(self, mcu_time):
|
def reset_step_clock(self, mcu_time):
|
||||||
clock = int(mcu_time * self._mcu_freq)
|
clock = int(mcu_time * self._mcu_freq)
|
||||||
ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
|
ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
|
||||||
|
@ -188,10 +194,7 @@ class MCU_endstop:
|
||||||
if not self._homing:
|
if not self._homing:
|
||||||
return False
|
return False
|
||||||
if not self._last_state.get('homing', 0):
|
if not self._last_state.get('homing', 0):
|
||||||
pos = self._last_state.get('pos', 0)
|
self._stepper.note_homing_triggered()
|
||||||
if self._stepper.get_invert_dir():
|
|
||||||
pos = -pos
|
|
||||||
self._stepper.set_mcu_position(pos)
|
|
||||||
self._homing = False
|
self._homing = False
|
||||||
return False
|
return False
|
||||||
if (self._mcu.serial.get_clock(last_sent_time)
|
if (self._mcu.serial.get_clock(last_sent_time)
|
||||||
|
|
|
@ -171,8 +171,8 @@ class SerialReader:
|
||||||
def encode_and_send(self, data, minclock, reqclock, cq):
|
def encode_and_send(self, data, minclock, reqclock, cq):
|
||||||
self.ffi_lib.serialqueue_encode_and_send(
|
self.ffi_lib.serialqueue_encode_and_send(
|
||||||
self.serialqueue, cq, data, len(data), minclock, reqclock)
|
self.serialqueue, cq, data, len(data), minclock, reqclock)
|
||||||
def send_with_response(self, cmd, name):
|
def send_with_response(self, cmd, name, oid=None):
|
||||||
src = SerialRetryCommand(self, cmd, name)
|
src = SerialRetryCommand(self, cmd, name, oid)
|
||||||
return src.get_response()
|
return src.get_response()
|
||||||
def alloc_command_queue(self):
|
def alloc_command_queue(self):
|
||||||
return self.ffi_main.gc(self.ffi_lib.serialqueue_alloc_commandqueue(),
|
return self.ffi_main.gc(self.ffi_lib.serialqueue_alloc_commandqueue(),
|
||||||
|
@ -235,17 +235,18 @@ class SerialReader:
|
||||||
class SerialRetryCommand:
|
class SerialRetryCommand:
|
||||||
TIMEOUT_TIME = 5.0
|
TIMEOUT_TIME = 5.0
|
||||||
RETRY_TIME = 0.500
|
RETRY_TIME = 0.500
|
||||||
def __init__(self, serial, cmd, name):
|
def __init__(self, serial, cmd, name, oid=None):
|
||||||
self.serial = serial
|
self.serial = serial
|
||||||
self.cmd = cmd
|
self.cmd = cmd
|
||||||
self.name = name
|
self.name = name
|
||||||
|
self.oid = oid
|
||||||
self.response = None
|
self.response = None
|
||||||
self.min_query_time = self.serial.reactor.monotonic()
|
self.min_query_time = self.serial.reactor.monotonic()
|
||||||
self.serial.register_callback(self.handle_callback, self.name)
|
self.serial.register_callback(self.handle_callback, self.name, self.oid)
|
||||||
self.send_timer = self.serial.reactor.register_timer(
|
self.send_timer = self.serial.reactor.register_timer(
|
||||||
self.send_event, self.serial.reactor.NOW)
|
self.send_event, self.serial.reactor.NOW)
|
||||||
def unregister(self):
|
def unregister(self):
|
||||||
self.serial.unregister_callback(self.name)
|
self.serial.unregister_callback(self.name, self.oid)
|
||||||
self.serial.reactor.unregister_timer(self.send_timer)
|
self.serial.reactor.unregister_timer(self.send_timer)
|
||||||
def send_event(self, eventtime):
|
def send_event(self, eventtime):
|
||||||
if self.response is not None:
|
if self.response is not None:
|
||||||
|
|
|
@ -42,7 +42,7 @@ void
|
||||||
command_config_end_stop(uint32_t *args)
|
command_config_end_stop(uint32_t *args)
|
||||||
{
|
{
|
||||||
struct end_stop *e = oid_alloc(args[0], command_config_end_stop, sizeof(*e));
|
struct end_stop *e = oid_alloc(args[0], command_config_end_stop, sizeof(*e));
|
||||||
struct stepper *s = oid_lookup(args[3], command_config_stepper);
|
struct stepper *s = stepper_oid_lookup(args[3]);
|
||||||
e->time.func = end_stop_event;
|
e->time.func = end_stop_event;
|
||||||
e->stepper = s;
|
e->stepper = s;
|
||||||
e->pin = gpio_in_setup(args[1], args[2]);
|
e->pin = gpio_in_setup(args[1], args[2]);
|
||||||
|
@ -73,14 +73,12 @@ static void
|
||||||
end_stop_report(uint8_t oid, struct end_stop *e)
|
end_stop_report(uint8_t oid, struct end_stop *e)
|
||||||
{
|
{
|
||||||
irq_disable();
|
irq_disable();
|
||||||
uint32_t position = stepper_get_position(e->stepper);
|
|
||||||
uint8_t eflags = e->flags;
|
uint8_t eflags = e->flags;
|
||||||
e->flags &= ~ESF_REPORT;
|
e->flags &= ~ESF_REPORT;
|
||||||
irq_enable();
|
irq_enable();
|
||||||
|
|
||||||
sendf("end_stop_state oid=%c homing=%c pin=%c pos=%i"
|
sendf("end_stop_state oid=%c homing=%c pin=%c"
|
||||||
, oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin)
|
, oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin));
|
||||||
, position - STEPPER_POSITION_BIAS);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
|
@ -46,6 +46,8 @@ struct stepper {
|
||||||
uint8_t flags : 8;
|
uint8_t flags : 8;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum { POSITION_BIAS=0x40000000 };
|
||||||
|
|
||||||
enum { SF_LAST_DIR=1<<0, SF_NEXT_DIR=1<<1, SF_INVERT_STEP=1<<2, SF_HAVE_ADD=1<<3,
|
enum { SF_LAST_DIR=1<<0, SF_NEXT_DIR=1<<1, SF_INVERT_STEP=1<<2, SF_HAVE_ADD=1<<3,
|
||||||
SF_LAST_RESET=1<<4, SF_NO_NEXT_CHECK=1<<5 };
|
SF_LAST_RESET=1<<4, SF_NO_NEXT_CHECK=1<<5 };
|
||||||
|
|
||||||
|
@ -152,18 +154,25 @@ command_config_stepper(uint32_t *args)
|
||||||
s->step_pin = gpio_out_setup(args[1], s->flags & SF_INVERT_STEP ? 1 : 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->dir_pin = gpio_out_setup(args[2], 0);
|
||||||
s->min_stop_interval = args[3];
|
s->min_stop_interval = args[3];
|
||||||
s->position = -STEPPER_POSITION_BIAS;
|
s->position = -POSITION_BIAS;
|
||||||
move_request_size(sizeof(struct stepper_move));
|
move_request_size(sizeof(struct stepper_move));
|
||||||
}
|
}
|
||||||
DECL_COMMAND(command_config_stepper,
|
DECL_COMMAND(command_config_stepper,
|
||||||
"config_stepper oid=%c step_pin=%c dir_pin=%c"
|
"config_stepper oid=%c step_pin=%c dir_pin=%c"
|
||||||
" min_stop_interval=%u invert_step=%c");
|
" min_stop_interval=%u invert_step=%c");
|
||||||
|
|
||||||
|
// Return the 'struct stepper' for a given stepper oid
|
||||||
|
struct stepper *
|
||||||
|
stepper_oid_lookup(uint8_t oid)
|
||||||
|
{
|
||||||
|
return oid_lookup(oid, command_config_stepper);
|
||||||
|
}
|
||||||
|
|
||||||
// Schedule a set of steps with a given timing
|
// Schedule a set of steps with a given timing
|
||||||
void
|
void
|
||||||
command_queue_step(uint32_t *args)
|
command_queue_step(uint32_t *args)
|
||||||
{
|
{
|
||||||
struct stepper *s = oid_lookup(args[0], command_config_stepper);
|
struct stepper *s = stepper_oid_lookup(args[0]);
|
||||||
struct stepper_move *m = move_alloc();
|
struct stepper_move *m = move_alloc();
|
||||||
m->interval = args[1];
|
m->interval = args[1];
|
||||||
m->count = args[2];
|
m->count = args[2];
|
||||||
|
@ -204,7 +213,7 @@ DECL_COMMAND(command_queue_step,
|
||||||
void
|
void
|
||||||
command_set_next_step_dir(uint32_t *args)
|
command_set_next_step_dir(uint32_t *args)
|
||||||
{
|
{
|
||||||
struct stepper *s = oid_lookup(args[0], command_config_stepper);
|
struct stepper *s = stepper_oid_lookup(args[0]);
|
||||||
uint8_t nextdir = args[1] ? SF_NEXT_DIR : 0;
|
uint8_t nextdir = args[1] ? SF_NEXT_DIR : 0;
|
||||||
irq_disable();
|
irq_disable();
|
||||||
s->flags = (s->flags & ~SF_NEXT_DIR) | nextdir;
|
s->flags = (s->flags & ~SF_NEXT_DIR) | nextdir;
|
||||||
|
@ -216,7 +225,7 @@ DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c");
|
||||||
void
|
void
|
||||||
command_reset_step_clock(uint32_t *args)
|
command_reset_step_clock(uint32_t *args)
|
||||||
{
|
{
|
||||||
struct stepper *s = oid_lookup(args[0], command_config_stepper);
|
struct stepper *s = stepper_oid_lookup(args[0]);
|
||||||
uint32_t waketime = args[1];
|
uint32_t waketime = args[1];
|
||||||
irq_disable();
|
irq_disable();
|
||||||
if (s->count)
|
if (s->count)
|
||||||
|
@ -228,7 +237,7 @@ command_reset_step_clock(uint32_t *args)
|
||||||
DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u");
|
DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u");
|
||||||
|
|
||||||
// Return the current stepper position. Caller must disable irqs.
|
// Return the current stepper position. Caller must disable irqs.
|
||||||
uint32_t
|
static uint32_t
|
||||||
stepper_get_position(struct stepper *s)
|
stepper_get_position(struct stepper *s)
|
||||||
{
|
{
|
||||||
uint32_t position = s->position;
|
uint32_t position = s->position;
|
||||||
|
@ -241,6 +250,19 @@ stepper_get_position(struct stepper *s)
|
||||||
return position;
|
return position;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Report the current position of the stepper
|
||||||
|
void
|
||||||
|
command_stepper_get_position(uint32_t *args)
|
||||||
|
{
|
||||||
|
uint8_t oid = args[0];
|
||||||
|
struct stepper *s = stepper_oid_lookup(oid);
|
||||||
|
irq_disable();
|
||||||
|
uint32_t position = stepper_get_position(s);
|
||||||
|
irq_enable();
|
||||||
|
sendf("stepper_position oid=%c pos=%i", oid, position - POSITION_BIAS);
|
||||||
|
}
|
||||||
|
DECL_COMMAND(command_stepper_get_position, "stepper_get_position oid=%c");
|
||||||
|
|
||||||
// Stop all moves for a given stepper (used in end stop homing). IRQs
|
// Stop all moves for a given stepper (used in end stop homing). IRQs
|
||||||
// must be off.
|
// must be off.
|
||||||
void
|
void
|
||||||
|
|
|
@ -3,12 +3,8 @@
|
||||||
|
|
||||||
#include <stdint.h> // uint8_t
|
#include <stdint.h> // uint8_t
|
||||||
|
|
||||||
enum { STEPPER_POSITION_BIAS=0x40000000 };
|
|
||||||
|
|
||||||
uint_fast8_t stepper_event(struct timer *t);
|
uint_fast8_t stepper_event(struct timer *t);
|
||||||
void command_config_stepper(uint32_t *args);
|
struct stepper *stepper_oid_lookup(uint8_t oid);
|
||||||
struct stepper;
|
|
||||||
uint32_t stepper_get_position(struct stepper *s);
|
|
||||||
void stepper_stop(struct stepper *s);
|
void stepper_stop(struct stepper *s);
|
||||||
|
|
||||||
#endif // stepper.h
|
#endif // stepper.h
|
||||||
|
|
Loading…
Reference in New Issue