3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

stepcompress: Merge stepcompress_push_accel() and stepcompress_push_const() 

It's not necessary to have separate C functions for constant
acceleration and constant velocity as constant velocity can be
obtained by using a constant acceleration of zero.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-07 12:51:52 -04:00
parent 1d81bf5596
commit 98add22891
6 changed files with 48 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
klippy/cartesian.py
View File

@ -112,18 +112,19 @@ class CartKinematics:
# Acceleration steps # Acceleration steps
if move.accel_r: if move.accel_r:
accel_d = move.accel_r * axis_d accel_d = move.accel_r * axis_d
mcu_stepper.step_accel( mcu_stepper.step_const(
mcu_time, start_pos, accel_d, move.start_v * axis_r, accel) mcu_time, start_pos, accel_d, move.start_v * axis_r, accel)
start_pos += accel_d start_pos += accel_d
mcu_time += move.accel_t mcu_time += move.accel_t
# Cruising steps # Cruising steps
if move.cruise_r: if move.cruise_r:
cruise_d = move.cruise_r * axis_d cruise_d = move.cruise_r * axis_d
mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v) mcu_stepper.step_const(
mcu_time, start_pos, cruise_d, cruise_v, 0.)
start_pos += cruise_d start_pos += cruise_d
mcu_time += move.cruise_t mcu_time += move.cruise_t
# Deceleration steps # Deceleration steps
if move.decel_r: if move.decel_r:
decel_d = move.decel_r * axis_d decel_d = move.decel_r * axis_d
mcu_stepper.step_accel( mcu_stepper.step_const(
mcu_time, start_pos, decel_d, cruise_v, -accel) mcu_time, start_pos, decel_d, cruise_v, -accel)

2
klippy/chelper.py
View File

@ -23,8 +23,6 @@ defs_stepcompress = """
int stepcompress_push(struct stepcompress *sc, double step_clock int stepcompress_push(struct stepcompress *sc, double step_clock
, int32_t sdir); , int32_t sdir);
int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
, double step_offset, double steps, double cruise_sv);
int32_t stepcompress_push_accel(struct stepcompress *sc, double clock_offset
, double step_offset, double steps, double start_sv, double accel); , double step_offset, double steps, double start_sv, double accel);
int32_t stepcompress_push_delta_const(struct stepcompress *sc int32_t stepcompress_push_delta_const(struct stepcompress *sc
, double clock_offset, double dist, double start_pos , double clock_offset, double dist, double start_pos

7
klippy/corexy.py
View File

@ -126,18 +126,19 @@ class CoreXYKinematics:
# Acceleration steps # Acceleration steps
if move.accel_r: if move.accel_r:
accel_d = move.accel_r * axis_d accel_d = move.accel_r * axis_d
mcu_stepper.step_accel( mcu_stepper.step_const(
mcu_time, start_pos, accel_d, move.start_v * axis_r, accel) mcu_time, start_pos, accel_d, move.start_v * axis_r, accel)
start_pos += accel_d start_pos += accel_d
mcu_time += move.accel_t mcu_time += move.accel_t
# Cruising steps # Cruising steps
if move.cruise_r: if move.cruise_r:
cruise_d = move.cruise_r * axis_d cruise_d = move.cruise_r * axis_d
mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v) mcu_stepper.step_const(
mcu_time, start_pos, cruise_d, cruise_v, 0.)
start_pos += cruise_d start_pos += cruise_d
mcu_time += move.cruise_t mcu_time += move.cruise_t
# Deceleration steps # Deceleration steps
if move.decel_r: if move.decel_r:
decel_d = move.decel_r * axis_d decel_d = move.decel_r * axis_d
mcu_stepper.step_accel( mcu_stepper.step_const(
mcu_time, start_pos, decel_d, cruise_v, -accel) mcu_time, start_pos, decel_d, cruise_v, -accel)

8
klippy/extruder.py
View File

@ -169,22 +169,22 @@ class PrinterExtruder:
# Acceleration steps # Acceleration steps
if accel_d: if accel_d:
mcu_stepper.step_accel(mcu_time, start_pos, accel_d, start_v, accel) mcu_stepper.step_const(mcu_time, start_pos, accel_d, start_v, accel)
start_pos += accel_d start_pos += accel_d
mcu_time += accel_t mcu_time += accel_t
# Cruising steps # Cruising steps
if cruise_d: if cruise_d:
mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v) mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v, 0.)
start_pos += cruise_d start_pos += cruise_d
mcu_time += cruise_t mcu_time += cruise_t
# Deceleration steps # Deceleration steps
if decel_d: if decel_d:
mcu_stepper.step_accel(mcu_time, start_pos, decel_d, decel_v, -accel) mcu_stepper.step_const(mcu_time, start_pos, decel_d, decel_v, -accel)
start_pos += decel_d start_pos += decel_d
mcu_time += decel_t mcu_time += decel_t
# Retraction steps # Retraction steps
if retract_d: if retract_d:
mcu_stepper.step_accel( mcu_stepper.step_const(
mcu_time, start_pos, -retract_d, retract_v, accel) mcu_time, start_pos, -retract_d, retract_v, accel)
start_pos -= retract_d start_pos -= retract_d
self.extrude_pos = start_pos self.extrude_pos = start_pos

11
klippy/mcu.py
View File

@ -118,19 +118,10 @@ class MCU_stepper:
self._commanded_pos += 1 self._commanded_pos += 1
else: else:
self._commanded_pos -= 1 self._commanded_pos -= 1
def step_const(self, mcu_time, start_pos, dist, cruise_v): def step_const(self, mcu_time, start_pos, dist, start_v, accel):
inv_step_dist = self._inv_step_dist inv_step_dist = self._inv_step_dist
step_offset = self._commanded_pos - start_pos * inv_step_dist step_offset = self._commanded_pos - start_pos * inv_step_dist
count = self._ffi_lib.stepcompress_push_const( count = self._ffi_lib.stepcompress_push_const(
self._stepqueue, mcu_time * self._mcu_freq, step_offset,
dist * inv_step_dist, cruise_v * self._velocity_factor)
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self._commanded_pos += count
def step_accel(self, mcu_time, start_pos, dist, start_v, accel):
inv_step_dist = self._inv_step_dist
step_offset = self._commanded_pos - start_pos * inv_step_dist
count = self._ffi_lib.stepcompress_push_accel(
self._stepqueue, mcu_time * self._mcu_freq, step_offset, self._stepqueue, mcu_time * self._mcu_freq, step_offset,
dist * inv_step_dist, start_v * self._velocity_factor, dist * inv_step_dist, start_v * self._velocity_factor,
accel * self._accel_factor) accel * self._accel_factor)

73
klippy/stepcompress.c
View File

@ -464,11 +464,16 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
return 0; return 0;
} }
// Schedule 'steps' number of steps with a constant time between steps // Schedule 'steps' number of steps at constant acceleration. If
// using the formula: step_clock = clock_offset + step_num/cruise_sv // acceleration is zero (ie, constant velocity) it uses the formula:
// step_clock = clock_offset + step_num/start_sv
// Otherwise it uses the formula:
// step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2)
// - start_sv/accel)
int32_t int32_t
stepcompress_push_const(struct stepcompress *sc, double clock_offset stepcompress_push_const(
, double step_offset, double steps, double cruise_sv) struct stepcompress *sc, double clock_offset
, double step_offset, double steps, double start_sv, double accel)
{ {
// Calculate number of steps to take // Calculate number of steps to take
int sdir = 1; int sdir = 1;
@ -480,8 +485,9 @@ stepcompress_push_const(struct stepcompress *sc, double clock_offset
int count = steps + .5 - step_offset; int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) { if (count <= 0 || count > 10000000) {
if (count && steps) { if (count && steps) {
errorf("push_const invalid count %d %f %f %f %f" errorf("push_const invalid count %d %f %f %f %f %f"
, sc->oid, clock_offset, step_offset, steps, cruise_sv); , sc->oid, clock_offset, step_offset, steps
, start_sv, accel);
return ERROR_RET; return ERROR_RET;
} }
return 0; return 0;
@ -493,64 +499,33 @@ stepcompress_push_const(struct stepcompress *sc, double clock_offset
// Calculate each step time // Calculate each step time
clock_offset += 0.5; clock_offset += 0.5;
double factor = 1. / cruise_sv;
double pos = step_offset + .5; double pos = step_offset + .5;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
if (!accel) {
// Move at constant velocity (zero acceleration)
double inv_cruise_sv = 1. / start_sv;
while (count--) { while (count--) {
int ret = check_expand(sc, &qn, &qend); int ret = check_expand(sc, &qn, &qend);
if (ret) if (ret)
return ret; return ret;
*qn++ = clock_offset + pos*factor; *qn++ = clock_offset + pos*inv_cruise_sv;
pos += 1.0; pos += 1.0;
} }
sc->queue_next = qn; } else {
return res; // Move with constant acceleration
}
// Schedule 'steps' number of steps at constant acceleration. It uses
// the formula:
// step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2)
// - start_sv/accel)
int32_t
stepcompress_push_accel(struct stepcompress *sc, double clock_offset
, double step_offset, double steps
, double start_sv, double accel)
{
// Calculate number of steps to take
int sdir = 1;
if (steps < 0) {
sdir = 0;
steps = -steps;
step_offset = -step_offset;
}
int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) {
if (count && steps) {
errorf("push_accel invalid count %d %f %f %f %f %f"
, sc->oid, clock_offset, step_offset, steps, start_sv, accel);
return ERROR_RET;
}
return 0;
}
int ret = set_next_step_dir(sc, sdir);
if (ret)
return ret;
int res = sdir ? count : -count;
// Calculate each step time
double inv_accel = 1. / accel; double inv_accel = 1. / accel;
double factor = 2. * inv_accel; clock_offset -= start_sv * inv_accel;
clock_offset += 0.5 - start_sv * inv_accel; pos += .5 * start_sv*start_sv * inv_accel;
double pos = step_offset + .5 + .5 * start_sv*start_sv * inv_accel; double accel_multiplier = 2. * inv_accel;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
while (count--) { while (count--) {
int ret = check_expand(sc, &qn, &qend); int ret = check_expand(sc, &qn, &qend);
if (ret) if (ret)
return ret; return ret;
double v = safe_sqrt(pos*factor); double v = safe_sqrt(pos * accel_multiplier);
*qn++ = clock_offset + (factor >= 0. ? v : -v); *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
pos += 1.0; pos += 1.0;
} }
}
sc->queue_next = qn; sc->queue_next = qn;
return res; return res;
} }