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

delta: Make it clear that a "virtual tower" is created 

The delta code calculates a "virtual tower" along the line of
movement.  Rework the variable names and comments to make it clear
that this is occurring.

It is not necessary to pass the start_pos variable to the C code as it
is simple to update the start_pos at the start of each movement.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-06 11:09:08 -04:00
parent 85ed5cef7f
commit b915a2ad7d
4 changed files with 52 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
klippy/chelper.py
View File

@ -25,9 +25,8 @@ defs_stepcompress = """
int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
, double step_offset, double steps, double start_sv, double accel);
int32_t stepcompress_push_delta(struct stepcompress *sc
, double clock_offset, double start_pos, double steps, double start_sv
, double accel, double height, double closestxy_sd
, double closest_height2, double movez_r);
, double clock_offset, double move_sd, double start_sv, double accel
, double height, double startxy_sd, double arm_d, double movez_r);
struct steppersync *steppersync_alloc(struct serialqueue *sq
, struct stepcompress **sc_list, int sc_num, int move_num);

48
klippy/delta.py
View File

@ -162,23 +162,24 @@ class DeltaKinematics:
limit_xy2 = -1.
self.limit_xy2 = min(limit_xy2, self.slow_xy2)
def move(self, move_time, move):
if self.need_motor_enable:
self._check_motor_enable(move_time)
axes_d = move.axes_d
move_d = movexy_d = move.move_d
movexy_r = 1.
movez_r = 0.
inv_movexy_d = 1. / movexy_d
if not axes_d[0] and not axes_d[1]:
# Z only move
movez_r = axes_d[2] * inv_movexy_d
movexy_d = movexy_r = inv_movexy_d = 0.
elif axes_d[2]:
# XY+Z move
movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
movexy_r = movexy_d * inv_movexy_d
movez_r = axes_d[2] * inv_movexy_d
inv_movexy_d = 1. / movexy_d
if self.need_motor_enable:
self._check_motor_enable(move_time)
origx, origy, origz = move.start_pos[:3]
accel = move.accel
@ -189,15 +190,16 @@ class DeltaKinematics:
cruise_end_d = accel_d + move.cruise_r * move_d
for i in StepList:
# Find point on line of movement closest to tower
# Calculate a virtual tower along the line of movement at
# the point closest to this stepper's tower.
towerx_d = self.towers[i][0] - origx
towery_d = self.towers[i][1] - origy
closestxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
tangentxy_d2 = towerx_d**2 + towery_d**2 - closestxy_d**2
closest_height2 = self.arm_length2 - tangentxy_d2
vt_startxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
tangentxy_d2 = towerx_d**2 + towery_d**2 - vt_startxy_d**2
vt_arm_d = math.sqrt(self.arm_length2 - tangentxy_d2)
# Calculate accel/cruise/decel portions of move
reversexy_d = closestxy_d + math.sqrt(closest_height2)*movez_r
reversexy_d = vt_startxy_d + vt_arm_d*movez_r
accel_up_d = cruise_up_d = decel_up_d = 0.
accel_down_d = cruise_down_d = decel_down_d = 0.
if reversexy_d <= 0.:
@ -229,30 +231,36 @@ class DeltaKinematics:
mcu_time = mcu_stepper.print_to_mcu_time(move_time)
if accel_up_d > 0.:
mcu_stepper.step_delta(
mcu_time, 0., accel_up_d, move.start_v, accel,
origz, closestxy_d, closest_height2, movez_r)
mcu_time, accel_up_d, move.start_v, accel,
origz, vt_startxy_d, vt_arm_d, movez_r)
if cruise_up_d > 0.:
mcu_stepper.step_delta(
mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, 0.,
origz, closestxy_d, closest_height2, movez_r)
mcu_time + accel_t, cruise_up_d - accel_d, cruise_v, 0.,
origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r,
vt_arm_d, movez_r)
if decel_up_d > 0.:
mcu_stepper.step_delta(
mcu_time + cruise_end_t, cruise_end_d, decel_up_d,
mcu_time + cruise_end_t, decel_up_d - cruise_end_d,
cruise_v, -accel,
origz, closestxy_d, closest_height2, movez_r)
origz + cruise_end_d*movez_r,
vt_startxy_d - cruise_end_d*movexy_r,
vt_arm_d, movez_r)
if accel_down_d > 0.:
mcu_stepper.step_delta(
mcu_time, 0., -accel_down_d, move.start_v, accel,
origz, closestxy_d, closest_height2, movez_r)
mcu_time, -accel_down_d, move.start_v, accel,
origz, vt_startxy_d, vt_arm_d, movez_r)
if cruise_down_d > 0.:
mcu_stepper.step_delta(
mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v, 0.,
origz, closestxy_d, closest_height2, movez_r)
mcu_time + accel_t, accel_d - cruise_down_d, cruise_v, 0.,
origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r,
vt_arm_d, movez_r)
if decel_down_d > 0.:
mcu_stepper.step_delta(
mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
mcu_time + cruise_end_t, cruise_end_d - decel_down_d,
cruise_v, -accel,
origz, closestxy_d, closest_height2, movez_r)
origz + cruise_end_d*movez_r,
vt_startxy_d - cruise_end_d*movexy_r,
vt_arm_d, movez_r)
######################################################################

10
klippy/mcu.py
View File

@ -128,16 +128,14 @@ class MCU_stepper:
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self._commanded_pos += count
def step_delta(self, mcu_time, start_pos, dist, start_v, accel
, height_base, closestxy_d, closest_height2, movez_r):
def step_delta(self, mcu_time, dist, start_v, accel
, height_base, startxy_d, arm_d, movez_r):
inv_step_dist = self._inv_step_dist
height = self._commanded_pos - height_base * inv_step_dist
count = self._ffi_lib.stepcompress_push_delta(
self._stepqueue, mcu_time * self._mcu_freq,
-start_pos * inv_step_dist, dist * inv_step_dist,
self._stepqueue, mcu_time * self._mcu_freq, dist * inv_step_dist,
start_v * self._velocity_factor, accel * self._accel_factor,
height, closestxy_d * inv_step_dist,
closest_height2 * inv_step_dist**2, movez_r)
height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r)
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self._commanded_pos += count

35
klippy/stepcompress.c
View File

@ -533,25 +533,26 @@ stepcompress_push_const(
// Schedule steps using delta kinematics
int32_t
stepcompress_push_delta(
struct stepcompress *sc, double clock_offset, double start_pos
, double steps, double start_sv, double accel
, double height, double closestxy_sd, double closest_height2, double movez_r)
struct stepcompress *sc, double clock_offset, double move_sd
, double start_sv, double accel
, double height, double startxy_sd, double arm_sd, double movez_r)
{
// Calculate number of steps to take
double step_dist = 1.;
if (steps < 0) {
if (move_sd < 0) {
step_dist = -1.;
steps = -steps;
move_sd = -move_sd;
}
double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
double reldist = closestxy_sd - movexy_r*steps;
double end_height = safe_sqrt(closest_height2 - reldist*reldist);
int count = (end_height - height + movez_r*steps) * step_dist + .5;
double arm_sd2 = arm_sd * arm_sd;
double endxy_sd = startxy_sd - movexy_r*move_sd;
double end_height = safe_sqrt(arm_sd2 - endxy_sd*endxy_sd);
int count = (end_height + movez_r*move_sd - height) * step_dist + .5;
if (count <= 0 || count > 10000000) {
if (count) {
errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f %f"
, sc->oid, count, clock_offset, start_pos, steps, start_sv
, accel, height, closestxy_sd, closest_height2, movez_r);
errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f"
, sc->oid, count, clock_offset, move_sd, start_sv, accel
, height, startxy_sd, arm_sd, movez_r);
return ERROR_RET;
}
return 0;
@ -563,7 +564,7 @@ stepcompress_push_delta(
// Calculate each step time
clock_offset += 0.5;
start_pos += movexy_r*closestxy_sd;
double start_pos = movexy_r*startxy_sd;
height += .5 * step_dist;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
if (!accel) {
@ -575,7 +576,7 @@ stepcompress_push_delta(
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
double v = safe_sqrt(closest_height2 - height*height);
double v = safe_sqrt(arm_sd2 - height*height);
double pos = start_pos + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_cruise_sv;
height += step_dist;
@ -597,8 +598,8 @@ stepcompress_push_delta(
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
double relheight = movexy_r*height - movez_r*closestxy_sd;
double v = safe_sqrt(closest_height2 - relheight*relheight);
double relheight = movexy_r*height - movez_r*startxy_sd;
double v = safe_sqrt(arm_sd2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_cruise_sv;
height += step_dist;
@ -614,8 +615,8 @@ stepcompress_push_delta(
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
double relheight = movexy_r*height - movez_r*closestxy_sd;
double v = safe_sqrt(closest_height2 - relheight*relheight);
double relheight = movexy_r*height - movez_r*startxy_sd;
double v = safe_sqrt(arm_sd2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
v = safe_sqrt(pos * accel_multiplier);
*qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);