stepcompress: Do all step rounding in C code
Commitsf0cefebf
and8f331f08
changed the way the code determined what steps to take on fractional steps. Unfortunately, it was possible in some situations for the C code to round differently from the python code which could result in warnings and lost steps. Change the code so that all fractional step handling is done in the C code. Implementing the step rounding logic in one location avoids any conflicts. In order to efficiently handle the step rounding in the C code, the C code has also been extended to directly send the "set_next_step_dir" command. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
79da35d023
commit
7554c7f694
|
@ -97,45 +97,47 @@ class CartKinematics:
|
||||||
inv_accel = 1. / move.accel
|
inv_accel = 1. / move.accel
|
||||||
inv_cruise_v = 1. / move.cruise_v
|
inv_cruise_v = 1. / move.cruise_v
|
||||||
for i in StepList:
|
for i in StepList:
|
||||||
inv_step_dist = self.steppers[i].inv_step_dist
|
if not move.axes_d[i]:
|
||||||
new_step_pos = int(move.end_pos[i]*inv_step_dist + 0.5)
|
|
||||||
step_pos = self.stepper_pos[i]
|
|
||||||
if new_step_pos == step_pos:
|
|
||||||
continue
|
continue
|
||||||
self.stepper_pos[i] = new_step_pos
|
mcu_time, so = self.steppers[i].prep_move(move_time)
|
||||||
|
inv_step_dist = self.steppers[i].inv_step_dist
|
||||||
steps = move.axes_d[i] * inv_step_dist
|
steps = move.axes_d[i] * inv_step_dist
|
||||||
step_offset = step_pos - move.start_pos[i] * inv_step_dist + 0.5
|
move_step_d = move.move_d / abs(steps)
|
||||||
sdir = 1
|
|
||||||
if steps < 0:
|
|
||||||
sdir = 0
|
|
||||||
steps = -steps
|
|
||||||
step_offset = 1. - step_offset
|
|
||||||
mcu_time, so = self.steppers[i].prep_move(move_time, sdir)
|
|
||||||
|
|
||||||
move_step_d = move.move_d / steps
|
step_pos = self.stepper_pos[i]
|
||||||
|
step_offset = step_pos - move.start_pos[i] * inv_step_dist
|
||||||
|
|
||||||
# Acceleration steps
|
# Acceleration steps
|
||||||
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
|
|
||||||
accel_time_offset = move.start_v * inv_accel
|
|
||||||
accel_sqrt_offset = accel_time_offset**2
|
|
||||||
accel_multiplier = 2.0 * move_step_d * inv_accel
|
accel_multiplier = 2.0 * move_step_d * inv_accel
|
||||||
accel_steps = move.accel_r * steps
|
if move.accel_r:
|
||||||
step_offset = so.step_sqrt(
|
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
|
||||||
mcu_time - accel_time_offset, accel_steps, step_offset
|
accel_time_offset = move.start_v * inv_accel
|
||||||
, accel_sqrt_offset, accel_multiplier)
|
accel_sqrt_offset = accel_time_offset**2
|
||||||
mcu_time += move.accel_t
|
accel_steps = move.accel_r * steps
|
||||||
|
count = so.step_sqrt(
|
||||||
|
mcu_time - accel_time_offset, accel_steps, step_offset
|
||||||
|
, accel_sqrt_offset, accel_multiplier)
|
||||||
|
step_pos += count
|
||||||
|
step_offset += count - accel_steps
|
||||||
|
mcu_time += move.accel_t
|
||||||
# Cruising steps
|
# Cruising steps
|
||||||
#t = pos/cruise_v
|
if move.cruise_r:
|
||||||
cruise_multiplier = move_step_d * inv_cruise_v
|
#t = pos/cruise_v
|
||||||
cruise_steps = move.cruise_r * steps
|
cruise_multiplier = move_step_d * inv_cruise_v
|
||||||
step_offset = so.step_factor(
|
cruise_steps = move.cruise_r * steps
|
||||||
mcu_time, cruise_steps, step_offset, cruise_multiplier)
|
count = so.step_factor(
|
||||||
mcu_time += move.cruise_t
|
mcu_time, cruise_steps, step_offset, cruise_multiplier)
|
||||||
|
step_pos += count
|
||||||
|
step_offset += count - cruise_steps
|
||||||
|
mcu_time += move.cruise_t
|
||||||
# Deceleration steps
|
# Deceleration steps
|
||||||
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
|
if move.decel_r:
|
||||||
decel_time_offset = move.cruise_v * inv_accel
|
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
|
||||||
decel_sqrt_offset = decel_time_offset**2
|
decel_time_offset = move.cruise_v * inv_accel
|
||||||
decel_steps = move.decel_r * steps
|
decel_sqrt_offset = decel_time_offset**2
|
||||||
so.step_sqrt(
|
decel_steps = move.decel_r * steps
|
||||||
mcu_time + decel_time_offset, decel_steps, step_offset
|
count = so.step_sqrt(
|
||||||
, decel_sqrt_offset, -accel_multiplier)
|
mcu_time + decel_time_offset, decel_steps, step_offset
|
||||||
|
, decel_sqrt_offset, -accel_multiplier)
|
||||||
|
step_pos += count
|
||||||
|
self.stepper_pos[i] = step_pos
|
||||||
|
|
|
@ -13,12 +13,14 @@ OTHER_FILES = ['list.h', 'serialqueue.h']
|
||||||
|
|
||||||
defs_stepcompress = """
|
defs_stepcompress = """
|
||||||
struct stepcompress *stepcompress_alloc(uint32_t max_error
|
struct stepcompress *stepcompress_alloc(uint32_t max_error
|
||||||
, uint32_t queue_step_msgid, uint32_t oid);
|
, uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
|
||||||
void stepcompress_push(struct stepcompress *sc, double step_clock);
|
, uint32_t invert_sdir, uint32_t oid);
|
||||||
double stepcompress_push_factor(struct stepcompress *sc
|
void stepcompress_push(struct stepcompress *sc, double step_clock
|
||||||
|
, int32_t sdir);
|
||||||
|
int32_t stepcompress_push_factor(struct stepcompress *sc
|
||||||
, double steps, double step_offset
|
, double steps, double step_offset
|
||||||
, double clock_offset, double factor);
|
, double clock_offset, double factor);
|
||||||
double stepcompress_push_sqrt(struct stepcompress *sc
|
int32_t stepcompress_push_sqrt(struct stepcompress *sc
|
||||||
, double steps, double step_offset
|
, double steps, double step_offset
|
||||||
, double clock_offset, double sqrt_offset, double factor);
|
, double clock_offset, double sqrt_offset, double factor);
|
||||||
void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
|
||||||
|
|
|
@ -88,69 +88,58 @@ class PrinterExtruder:
|
||||||
# There is still only a decel phase (no retraction)
|
# There is still only a decel phase (no retraction)
|
||||||
decel_d -= extra_decel_d
|
decel_d -= extra_decel_d
|
||||||
|
|
||||||
# Determine regular steps
|
# Prepare for steps
|
||||||
forward_d = accel_d + cruise_d + decel_d
|
stepper_pos = self.stepper_pos
|
||||||
end_pos = start_pos + forward_d
|
|
||||||
inv_step_dist = self.stepper.inv_step_dist
|
inv_step_dist = self.stepper.inv_step_dist
|
||||||
new_step_pos = int(end_pos*inv_step_dist + 0.5)
|
step_dist = self.stepper.step_dist
|
||||||
if new_step_pos != self.stepper_pos:
|
mcu_time, so = self.stepper.prep_move(move_time)
|
||||||
steps = forward_d * inv_step_dist
|
step_offset = stepper_pos - start_pos * inv_step_dist
|
||||||
step_offset = self.stepper_pos - start_pos * inv_step_dist + 0.5
|
|
||||||
self.stepper_pos = new_step_pos
|
|
||||||
sdir = 1
|
|
||||||
if steps < 0:
|
|
||||||
sdir = 0
|
|
||||||
steps = -steps
|
|
||||||
step_offset = 1. - step_offset
|
|
||||||
mcu_time, so = self.stepper.prep_move(move_time, sdir)
|
|
||||||
|
|
||||||
move_step_d = forward_d / steps
|
# Acceleration steps
|
||||||
inv_move_step_d = 1. / move_step_d
|
accel_multiplier = 2.0 * step_dist * inv_accel
|
||||||
|
if accel_d:
|
||||||
# Acceleration steps
|
|
||||||
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
|
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
|
||||||
accel_time_offset = start_v * inv_accel
|
accel_time_offset = start_v * inv_accel
|
||||||
accel_sqrt_offset = accel_time_offset**2
|
accel_sqrt_offset = accel_time_offset**2
|
||||||
accel_multiplier = 2.0 * move_step_d * inv_accel
|
accel_steps = accel_d * inv_step_dist
|
||||||
accel_steps = accel_d * inv_move_step_d
|
count = so.step_sqrt(
|
||||||
step_offset = so.step_sqrt(
|
|
||||||
mcu_time - accel_time_offset, accel_steps, step_offset
|
mcu_time - accel_time_offset, accel_steps, step_offset
|
||||||
, accel_sqrt_offset, accel_multiplier)
|
, accel_sqrt_offset, accel_multiplier)
|
||||||
|
stepper_pos += count
|
||||||
|
step_offset += count - accel_steps
|
||||||
mcu_time += accel_t
|
mcu_time += accel_t
|
||||||
# Cruising steps
|
# Cruising steps
|
||||||
|
if cruise_d:
|
||||||
#t = pos/cruise_v
|
#t = pos/cruise_v
|
||||||
cruise_multiplier = move_step_d / cruise_v
|
cruise_multiplier = step_dist / cruise_v
|
||||||
cruise_steps = cruise_d * inv_move_step_d
|
cruise_steps = cruise_d * inv_step_dist
|
||||||
step_offset = so.step_factor(
|
count = so.step_factor(
|
||||||
mcu_time, cruise_steps, step_offset, cruise_multiplier)
|
mcu_time, cruise_steps, step_offset, cruise_multiplier)
|
||||||
|
stepper_pos += count
|
||||||
|
step_offset += count - cruise_steps
|
||||||
mcu_time += cruise_t
|
mcu_time += cruise_t
|
||||||
# Deceleration steps
|
# Deceleration steps
|
||||||
|
if decel_d:
|
||||||
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
|
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
|
||||||
decel_time_offset = decel_v * inv_accel
|
decel_time_offset = decel_v * inv_accel
|
||||||
decel_sqrt_offset = decel_time_offset**2
|
decel_sqrt_offset = decel_time_offset**2
|
||||||
decel_steps = decel_d * inv_move_step_d
|
decel_steps = decel_d * inv_step_dist
|
||||||
so.step_sqrt(
|
count = so.step_sqrt(
|
||||||
mcu_time + decel_time_offset, decel_steps, step_offset
|
mcu_time + decel_time_offset, decel_steps, step_offset
|
||||||
, decel_sqrt_offset, -accel_multiplier)
|
, decel_sqrt_offset, -accel_multiplier)
|
||||||
|
stepper_pos += count
|
||||||
# Determine retract steps
|
step_offset += count - decel_steps
|
||||||
start_pos = end_pos
|
mcu_time += decel_t
|
||||||
end_pos -= retract_d
|
# Retraction steps
|
||||||
new_step_pos = int(end_pos*inv_step_dist + 0.5)
|
if retract_d:
|
||||||
if new_step_pos != self.stepper_pos:
|
|
||||||
steps = retract_d * inv_step_dist
|
|
||||||
step_offset = start_pos * inv_step_dist - self.stepper_pos + 0.5
|
|
||||||
self.stepper_pos = new_step_pos
|
|
||||||
mcu_time, so = self.stepper.prep_move(
|
|
||||||
move_time+accel_t+cruise_t+decel_t, 0)
|
|
||||||
|
|
||||||
move_step_d = retract_d / steps
|
|
||||||
|
|
||||||
# Acceleration steps
|
|
||||||
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
|
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
|
||||||
accel_time_offset = retract_v * inv_accel
|
accel_time_offset = retract_v * inv_accel
|
||||||
accel_sqrt_offset = accel_time_offset**2
|
accel_sqrt_offset = accel_time_offset**2
|
||||||
accel_multiplier = 2.0 * move_step_d * inv_accel
|
accel_steps = -retract_d * inv_step_dist
|
||||||
so.step_sqrt(mcu_time - accel_time_offset, steps, step_offset
|
count = so.step_sqrt(
|
||||||
, accel_sqrt_offset, accel_multiplier)
|
mcu_time - accel_time_offset, accel_steps, step_offset
|
||||||
self.extrude_pos = end_pos
|
, accel_sqrt_offset, accel_multiplier)
|
||||||
|
stepper_pos += count
|
||||||
|
|
||||||
|
self.stepper_pos = stepper_pos
|
||||||
|
self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d
|
||||||
|
|
|
@ -22,7 +22,7 @@ class MCU_stepper:
|
||||||
self._oid = mcu.create_oid()
|
self._oid = mcu.create_oid()
|
||||||
step_pin, pullup, invert_step = parse_pin_extras(step_pin)
|
step_pin, pullup, invert_step = parse_pin_extras(step_pin)
|
||||||
dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
|
dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
|
||||||
self._sdir = -1
|
self._need_reset = True
|
||||||
self._mcu_freq = mcu.get_mcu_freq()
|
self._mcu_freq = mcu.get_mcu_freq()
|
||||||
min_stop_interval = int(min_stop_interval * self._mcu_freq)
|
min_stop_interval = int(min_stop_interval * self._mcu_freq)
|
||||||
max_error = int(max_error * self._mcu_freq)
|
max_error = int(max_error * self._mcu_freq)
|
||||||
|
@ -39,30 +39,26 @@ class MCU_stepper:
|
||||||
"reset_step_clock oid=%c clock=%u")
|
"reset_step_clock oid=%c clock=%u")
|
||||||
ffi_main, self.ffi_lib = chelper.get_ffi()
|
ffi_main, self.ffi_lib = chelper.get_ffi()
|
||||||
self._stepqueue = self.ffi_lib.stepcompress_alloc(
|
self._stepqueue = self.ffi_lib.stepcompress_alloc(
|
||||||
max_error, self._step_cmd.msgid, self._oid)
|
max_error, self._step_cmd.msgid
|
||||||
|
, self._dir_cmd.msgid, self._invert_dir, self._oid)
|
||||||
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):
|
def get_invert_dir(self):
|
||||||
return self._invert_dir
|
return self._invert_dir
|
||||||
def note_stepper_stop(self):
|
def note_stepper_stop(self):
|
||||||
self._sdir = -1
|
self._need_reset = True
|
||||||
def _reset_step_clock(self, clock):
|
def check_reset(self, mcu_time):
|
||||||
|
if not self._need_reset:
|
||||||
|
return
|
||||||
|
self._need_reset = False
|
||||||
|
clock = int(mcu_time * self._mcu_freq)
|
||||||
self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
|
self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
|
||||||
data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
|
data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
|
||||||
self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
|
self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
|
||||||
def set_next_step_dir(self, mcu_time, sdir):
|
def step(self, mcu_time, sdir):
|
||||||
if self._sdir == sdir:
|
|
||||||
return
|
|
||||||
if self._sdir == -1:
|
|
||||||
clock = int(mcu_time * self._mcu_freq)
|
|
||||||
self._reset_step_clock(clock)
|
|
||||||
self._sdir = sdir
|
|
||||||
data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir)
|
|
||||||
self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
|
|
||||||
def step(self, mcu_time):
|
|
||||||
clock = mcu_time * self._mcu_freq
|
clock = mcu_time * self._mcu_freq
|
||||||
self.ffi_lib.stepcompress_push(self._stepqueue, clock)
|
self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
|
||||||
def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
|
def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
|
||||||
clock = mcu_time * self._mcu_freq
|
clock = mcu_time * self._mcu_freq
|
||||||
mcu_freq2 = self._mcu_freq**2
|
mcu_freq2 = self._mcu_freq**2
|
||||||
|
@ -477,7 +473,7 @@ class Dummy_MCU_stepper:
|
||||||
self._stepid, dirstr, countstr, addstr, interval))
|
self._stepid, dirstr, countstr, addstr, interval))
|
||||||
def set_next_step_dir(self, dir):
|
def set_next_step_dir(self, dir):
|
||||||
self._sdir = dir
|
self._sdir = dir
|
||||||
def _reset_step_clock(self, clock):
|
def check_reset(self, clock):
|
||||||
self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock))
|
self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock))
|
||||||
def print_to_mcu_time(self, print_time):
|
def print_to_mcu_time(self, print_time):
|
||||||
return self._mcu.print_to_mcu_time(print_time)
|
return self._mcu.print_to_mcu_time(print_time)
|
||||||
|
|
|
@ -35,7 +35,8 @@ struct stepcompress {
|
||||||
// Message generation
|
// Message generation
|
||||||
uint64_t last_step_clock;
|
uint64_t last_step_clock;
|
||||||
struct list_head msg_queue;
|
struct list_head msg_queue;
|
||||||
uint32_t queue_step_msgid, oid;
|
uint32_t queue_step_msgid, set_next_step_dir_msgid, oid;
|
||||||
|
int sdir, invert_sdir;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -268,14 +269,19 @@ safe_sqrt(double v)
|
||||||
|
|
||||||
// Allocate a new 'stepcompress' object
|
// Allocate a new 'stepcompress' object
|
||||||
struct stepcompress *
|
struct stepcompress *
|
||||||
stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid, uint32_t oid)
|
stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid
|
||||||
|
, uint32_t set_next_step_dir_msgid, uint32_t invert_sdir
|
||||||
|
, uint32_t oid)
|
||||||
{
|
{
|
||||||
struct stepcompress *sc = malloc(sizeof(*sc));
|
struct stepcompress *sc = malloc(sizeof(*sc));
|
||||||
memset(sc, 0, sizeof(*sc));
|
memset(sc, 0, sizeof(*sc));
|
||||||
sc->max_error = max_error;
|
sc->max_error = max_error;
|
||||||
list_init(&sc->msg_queue);
|
list_init(&sc->msg_queue);
|
||||||
sc->queue_step_msgid = queue_step_msgid;
|
sc->queue_step_msgid = queue_step_msgid;
|
||||||
|
sc->set_next_step_dir_msgid = set_next_step_dir_msgid;
|
||||||
sc->oid = oid;
|
sc->oid = oid;
|
||||||
|
sc->sdir = -1;
|
||||||
|
sc->invert_sdir = !!invert_sdir;
|
||||||
return sc;
|
return sc;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -312,75 +318,102 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Send the set_next_step_dir command
|
||||||
|
static void
|
||||||
|
set_next_step_dir(struct stepcompress *sc, int sdir)
|
||||||
|
{
|
||||||
|
sc->sdir = sdir;
|
||||||
|
stepcompress_flush(sc, UINT64_MAX);
|
||||||
|
uint32_t msg[3] = {
|
||||||
|
sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir
|
||||||
|
};
|
||||||
|
struct queue_message *qm = message_alloc_and_encode(msg, 3);
|
||||||
|
qm->req_clock = sc->last_step_clock;
|
||||||
|
list_add_tail(&qm->node, &sc->msg_queue);
|
||||||
|
}
|
||||||
|
|
||||||
// Check if the internal queue needs to be expanded, and expand if so
|
// Check if the internal queue needs to be expanded, and expand if so
|
||||||
static inline void
|
static inline void
|
||||||
check_expand(struct stepcompress *sc, int count)
|
check_expand(struct stepcompress *sc, int sdir, int count)
|
||||||
{
|
{
|
||||||
|
if (sdir != sc->sdir)
|
||||||
|
set_next_step_dir(sc, sdir);
|
||||||
if (sc->queue_next + count > sc->queue_end)
|
if (sc->queue_next + count > sc->queue_end)
|
||||||
expand_queue(sc, count);
|
expand_queue(sc, count);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Schedule a step event at the specified step_clock time
|
// Schedule a step event at the specified step_clock time
|
||||||
void
|
void
|
||||||
stepcompress_push(struct stepcompress *sc, double step_clock)
|
stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
|
||||||
{
|
{
|
||||||
check_expand(sc, 1);
|
sdir = !!sdir;
|
||||||
|
check_expand(sc, sdir, 1);
|
||||||
step_clock += 0.5;
|
step_clock += 0.5;
|
||||||
*sc->queue_next++ = step_clock;
|
*sc->queue_next++ = step_clock;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Schedule 'steps' number of steps with a constant time between steps
|
// Schedule 'steps' number of steps with a constant time between steps
|
||||||
// using the formula: step_clock = clock_offset + step_num*factor
|
// using the formula: step_clock = clock_offset + step_num*factor
|
||||||
double
|
int32_t
|
||||||
stepcompress_push_factor(struct stepcompress *sc
|
stepcompress_push_factor(struct stepcompress *sc
|
||||||
, double steps, double step_offset
|
, double steps, double step_offset
|
||||||
, double clock_offset, double factor)
|
, double clock_offset, double factor)
|
||||||
{
|
{
|
||||||
// Calculate number of steps to take
|
// Calculate number of steps to take
|
||||||
double ceil_steps = ceil(steps - step_offset);
|
int sdir = 1;
|
||||||
double next_step_offset = ceil_steps - (steps - step_offset);
|
if (steps < 0) {
|
||||||
int count = ceil_steps;
|
sdir = 0;
|
||||||
if (count < 0 || count > 1000000) {
|
steps = -steps;
|
||||||
fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n"
|
step_offset = -step_offset;
|
||||||
, sc->oid, steps, step_offset, clock_offset, factor);
|
|
||||||
return next_step_offset;
|
|
||||||
}
|
}
|
||||||
check_expand(sc, count);
|
int count = steps + .5 - step_offset;
|
||||||
|
if (count <= 0 || count > 1000000) {
|
||||||
|
if (count && steps)
|
||||||
|
fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n"
|
||||||
|
, sc->oid, steps, step_offset, clock_offset, factor);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
check_expand(sc, sdir, count);
|
||||||
|
|
||||||
// Calculate each step time
|
// Calculate each step time
|
||||||
uint64_t *qn = sc->queue_next, *end = &qn[count];
|
uint64_t *qn = sc->queue_next, *end = &qn[count];
|
||||||
clock_offset += 0.5;
|
clock_offset += 0.5;
|
||||||
double pos = step_offset;
|
double pos = step_offset + .5;
|
||||||
while (qn < end) {
|
while (qn < end) {
|
||||||
*qn++ = clock_offset + pos*factor;
|
*qn++ = clock_offset + pos*factor;
|
||||||
pos += 1.0;
|
pos += 1.0;
|
||||||
}
|
}
|
||||||
sc->queue_next = qn;
|
sc->queue_next = qn;
|
||||||
return next_step_offset;
|
return sdir ? count : -count;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Schedule 'steps' number of steps using the formula:
|
// Schedule 'steps' number of steps using the formula:
|
||||||
// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
|
// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
|
||||||
double
|
int32_t
|
||||||
stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
|
stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
|
||||||
, double clock_offset, double sqrt_offset, double factor)
|
, double clock_offset, double sqrt_offset, double factor)
|
||||||
{
|
{
|
||||||
// Calculate number of steps to take
|
// Calculate number of steps to take
|
||||||
double ceil_steps = ceil(steps - step_offset);
|
int sdir = 1;
|
||||||
double next_step_offset = ceil_steps - (steps - step_offset);
|
if (steps < 0) {
|
||||||
int count = ceil_steps;
|
sdir = 0;
|
||||||
if (count < 0 || count > 1000000) {
|
steps = -steps;
|
||||||
fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n"
|
step_offset = -step_offset;
|
||||||
, sc->oid, steps, step_offset, clock_offset, sqrt_offset
|
|
||||||
, factor);
|
|
||||||
return next_step_offset;
|
|
||||||
}
|
}
|
||||||
check_expand(sc, count);
|
int count = steps + .5 - step_offset;
|
||||||
|
if (count <= 0 || count > 1000000) {
|
||||||
|
if (count && steps)
|
||||||
|
fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n"
|
||||||
|
, sc->oid, steps, step_offset, clock_offset, sqrt_offset
|
||||||
|
, factor);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
check_expand(sc, sdir, count);
|
||||||
|
|
||||||
// Calculate each step time
|
// Calculate each step time
|
||||||
uint64_t *qn = sc->queue_next, *end = &qn[count];
|
uint64_t *qn = sc->queue_next, *end = &qn[count];
|
||||||
clock_offset += 0.5;
|
clock_offset += 0.5;
|
||||||
double pos = step_offset + sqrt_offset/factor;
|
double pos = step_offset + .5 + sqrt_offset/factor;
|
||||||
if (factor >= 0.0)
|
if (factor >= 0.0)
|
||||||
while (qn < end) {
|
while (qn < end) {
|
||||||
*qn++ = clock_offset + safe_sqrt(pos*factor);
|
*qn++ = clock_offset + safe_sqrt(pos*factor);
|
||||||
|
@ -392,7 +425,7 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
|
||||||
pos += 1.0;
|
pos += 1.0;
|
||||||
}
|
}
|
||||||
sc->queue_next = qn;
|
sc->queue_next = qn;
|
||||||
return next_step_offset;
|
return sdir ? count : -count;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the internal state of the stepcompress object
|
// Reset the internal state of the stepcompress object
|
||||||
|
@ -401,6 +434,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
|
||||||
{
|
{
|
||||||
stepcompress_flush(sc, UINT64_MAX);
|
stepcompress_flush(sc, UINT64_MAX);
|
||||||
sc->last_step_clock = last_step_clock;
|
sc->last_step_clock = last_step_clock;
|
||||||
|
sc->sdir = -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Queue an mcu command to go out in order with stepper commands
|
// Queue an mcu command to go out in order with stepper commands
|
||||||
|
|
|
@ -69,9 +69,9 @@ class PrinterStepper:
|
||||||
mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
|
mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
|
||||||
self.mcu_enable.set_digital(mcu_time, enable)
|
self.mcu_enable.set_digital(mcu_time, enable)
|
||||||
self.need_motor_enable = True
|
self.need_motor_enable = True
|
||||||
def prep_move(self, move_time, sdir):
|
def prep_move(self, move_time):
|
||||||
mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
|
mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
|
||||||
self.mcu_stepper.set_next_step_dir(mcu_time, sdir)
|
self.mcu_stepper.check_reset(mcu_time)
|
||||||
if self.need_motor_enable:
|
if self.need_motor_enable:
|
||||||
self.motor_enable(move_time, 1)
|
self.motor_enable(move_time, 1)
|
||||||
self.need_motor_enable = False
|
self.need_motor_enable = False
|
||||||
|
|
Loading…
Reference in New Issue