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:
Kevin O'Connor 2017-03-05 15:00:15 -05:00
parent 8d6ecd9af8
commit a38437f378
6 changed files with 54 additions and 28 deletions

View File

@ -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

View File

@ -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)

View File

@ -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:

View File

@ -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

View File

@ -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

View File

@ -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