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

delta: Rework delta math to avoid using inv_movexy_r 

Taking the inverse of the XY move distance can lead to extremely large
values when the XY distance is very small.  This can lead to
saturation of the double precision variables and incorrect results.

Rework the delta kinematic math to avoid using this inverse.  Pass the
closestxy_d value directly to the C functions so that the C code can
calculate its intermediate constants.

After this change the move_z special case is no longer necessary as
the regular delta functions now work with movexy_r=0 and movez_r=1.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2016-12-04 19:30:35 -05:00
parent 5458f3cbd2
commit 9c932ad514
4 changed files with 83 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
klippy/chelper.py
View File

@ -24,14 +24,14 @@ defs_stepcompress = """
int32_t 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);
int32_t stepcompress_push_delta_const( int32_t stepcompress_push_delta_const(struct stepcompress *sc
struct stepcompress *sc, double clock_offset, double dist , double clock_offset, double dist, double start_pos
, double step_dist, double start_pos, double closest_height2 , double inv_velocity, double step_dist, double height
, double height, double movez_r, double inv_velocity); , double closestxy_d, double closest_height2, double movez_r);
int32_t stepcompress_push_delta_accel( int32_t stepcompress_push_delta_accel(struct stepcompress *sc
struct stepcompress *sc, double clock_offset, double dist , double clock_offset, double dist, double start_pos
, double step_dist, double start_pos, double closest_height2 , double accel_multiplier, double step_dist, double height
, double height, double movez_r, double accel_multiplier); , double closestxy_d, double closest_height2, double movez_r);
void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
void stepcompress_queue_msg(struct stepcompress *sc void stepcompress_queue_msg(struct stepcompress *sc
, uint32_t *data, int len); , uint32_t *data, int len);

133
klippy/delta.py
View File

@ -125,72 +125,25 @@ class DeltaKinematics:
raise homing.EndstopMoveError(end_pos) raise homing.EndstopMoveError(end_pos)
if move.axes_d[2]: if move.axes_d[2]:
move.limit_speed(self.max_z_velocity, 9999999.9) move.limit_speed(self.max_z_velocity, 9999999.9)
def move_z(self, move_time, move):
if not move.axes_d[2]:
return
if self.need_motor_enable:
self.check_motor_enable(move_time)
inv_accel = 1. / move.accel
inv_cruise_v = 1. / move.cruise_v
for i in StepList:
towerx_d = self.towers[i][0] - move.start_pos[0]
towery_d = self.towers[i][1] - move.start_pos[1]
tower_d2 = towerx_d**2 + towery_d**2
height = math.sqrt(self.arm_length2 - tower_d2) + move.start_pos[2]
mcu_stepper = self.steppers[i].mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time)
step_pos = mcu_stepper.commanded_position
inv_step_dist = self.steppers[i].inv_step_dist
step_offset = step_pos - height * inv_step_dist
step_dist = self.steppers[i].step_dist
steps = move.axes_d[2] * inv_step_dist
# Acceleration steps
accel_multiplier = 2.0 * step_dist * inv_accel
if move.accel_r:
#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_steps = move.accel_r * steps
count = mcu_stepper.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier)
step_offset += count - accel_steps
mcu_time += move.accel_t
# Cruising steps
if move.cruise_r:
#t = pos/cruise_v
cruise_multiplier = step_dist * inv_cruise_v
cruise_steps = move.cruise_r * steps
count = mcu_stepper.step_factor(
mcu_time, cruise_steps, step_offset, cruise_multiplier)
step_offset += count - cruise_steps
mcu_time += move.cruise_t
# Deceleration steps
if move.decel_r:
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
decel_time_offset = move.cruise_v * inv_accel
decel_sqrt_offset = decel_time_offset**2
decel_steps = move.decel_r * steps
count = mcu_stepper.step_sqrt(
mcu_time + decel_time_offset, decel_steps, step_offset
, decel_sqrt_offset, -accel_multiplier)
def move(self, move_time, move): def move(self, move_time, move):
axes_d = move.axes_d 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]: if not axes_d[0] and not axes_d[1]:
self.move_z(move_time, move) if not axes_d[2]:
return return
movez_r = axes_d[2] * inv_movexy_d
movexy_d = movexy_r = inv_movexy_d = 0.
elif axes_d[2]:
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: if self.need_motor_enable:
self.check_motor_enable(move_time) self.check_motor_enable(move_time)
move_d = move.move_d
movez_r = 0.
inv_movexy_d = 1. / move_d
inv_movexy_r = 1.
if axes_d[2]:
movez_r = axes_d[2] * inv_movexy_d
inv_movexy_d = 1. / math.sqrt(axes_d[0]**2 + axes_d[1]**2)
inv_movexy_r = move_d * inv_movexy_d
origx, origy, origz = move.start_pos[:3] origx, origy, origz = move.start_pos[:3]
@ -214,80 +167,76 @@ class DeltaKinematics:
closestxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d 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 tangentxy_d2 = towerx_d**2 + towery_d**2 - closestxy_d**2
closest_height2 = self.arm_length2 - tangentxy_d2 closest_height2 = self.arm_length2 - tangentxy_d2
closest_height = math.sqrt(closest_height2)
closest_d = closestxy_d * inv_movexy_r
closestz = origz + closest_d*movez_r
# Calculate accel/cruise/decel portions of move # Calculate accel/cruise/decel portions of move
reverse_d = closest_d + closest_height*movez_r*inv_movexy_r reversexy_d = closestxy_d + math.sqrt(closest_height2)*movez_r
accel_up_d = cruise_up_d = decel_up_d = 0. accel_up_d = cruise_up_d = decel_up_d = 0.
accel_down_d = cruise_down_d = decel_down_d = 0. accel_down_d = cruise_down_d = decel_down_d = 0.
if reverse_d <= 0.: if reversexy_d <= 0.:
accel_down_d = accel_d accel_down_d = accel_d
cruise_down_d = cruise_end_d cruise_down_d = cruise_end_d
decel_down_d = move_d decel_down_d = move_d
elif reverse_d >= move_d: elif reversexy_d >= movexy_d:
accel_up_d = accel_d accel_up_d = accel_d
cruise_up_d = cruise_end_d cruise_up_d = cruise_end_d
decel_up_d = move_d decel_up_d = move_d
elif reverse_d < accel_d: elif reversexy_d < accel_d * movexy_r:
accel_up_d = reverse_d accel_up_d = reversexy_d * move_d * inv_movexy_d
accel_down_d = accel_d accel_down_d = accel_d
cruise_down_d = cruise_end_d cruise_down_d = cruise_end_d
decel_down_d = move_d decel_down_d = move_d
elif reverse_d < cruise_end_d: elif reversexy_d < cruise_end_d * movexy_r:
accel_up_d = accel_d accel_up_d = accel_d
cruise_up_d = reverse_d cruise_up_d = reversexy_d * move_d * inv_movexy_d
cruise_down_d = cruise_end_d cruise_down_d = cruise_end_d
decel_down_d = move_d decel_down_d = move_d
else: else:
accel_up_d = accel_d accel_up_d = accel_d
cruise_up_d = cruise_end_d cruise_up_d = cruise_end_d
decel_up_d = reverse_d decel_up_d = reversexy_d * move_d * inv_movexy_d
decel_down_d = move_d decel_down_d = move_d
# Generate steps # Generate steps
mcu_stepper = self.steppers[i].mcu_stepper mcu_stepper = self.steppers[i].mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time) mcu_time = mcu_stepper.print_to_mcu_time(move_time)
step_pos = mcu_stepper.commanded_position step_pos = mcu_stepper.commanded_position
inv_step_dist = self.steppers[i].inv_step_dist
step_dist = self.steppers[i].step_dist step_dist = self.steppers[i].step_dist
height = step_pos*step_dist - closestz height = step_pos*step_dist - origz
if accel_up_d > 0.: if accel_up_d > 0.:
count = mcu_stepper.step_delta_accel( count = mcu_stepper.step_delta_accel(
mcu_time - accel_time_offset, closest_d - accel_up_d, mcu_time - accel_time_offset, accel_up_d,
step_dist, closest_d + accel_offset, accel_offset, accel_multiplier, step_dist,
closest_height2, height, movez_r, accel_multiplier) height, closestxy_d, closest_height2, movez_r)
height += count * step_dist height += count * step_dist
if cruise_up_d > 0.: if cruise_up_d > 0.:
count = mcu_stepper.step_delta_const( count = mcu_stepper.step_delta_const(
mcu_time + accel_t, closest_d - cruise_up_d, mcu_time + accel_t, cruise_up_d,
step_dist, closest_d - accel_d, -accel_d, inv_cruise_v, step_dist,
closest_height2, height, movez_r, inv_cruise_v) height, closestxy_d, closest_height2, movez_r)
height += count * step_dist height += count * step_dist
if decel_up_d > 0.: if decel_up_d > 0.:
count = mcu_stepper.step_delta_accel( count = mcu_stepper.step_delta_accel(
mcu_time + decel_time_offset, closest_d - decel_up_d, mcu_time + decel_time_offset, decel_up_d,
step_dist, closest_d - decel_offset, -decel_offset, -accel_multiplier, step_dist,
closest_height2, height, movez_r, -accel_multiplier) height, closestxy_d, closest_height2, movez_r)
height += count * step_dist height += count * step_dist
if accel_down_d > 0.: if accel_down_d > 0.:
count = mcu_stepper.step_delta_accel( count = mcu_stepper.step_delta_accel(
mcu_time - accel_time_offset, closest_d - accel_down_d, mcu_time - accel_time_offset, accel_down_d,
-step_dist, closest_d + accel_offset, accel_offset, accel_multiplier, -step_dist,
closest_height2, height, movez_r, accel_multiplier) height, closestxy_d, closest_height2, movez_r)
height += count * step_dist height += count * step_dist
if cruise_down_d > 0.: if cruise_down_d > 0.:
count = mcu_stepper.step_delta_const( count = mcu_stepper.step_delta_const(
mcu_time + accel_t, closest_d - cruise_down_d, mcu_time + accel_t, cruise_down_d,
-step_dist, closest_d - accel_d, -accel_d, inv_cruise_v, -step_dist,
closest_height2, height, movez_r, inv_cruise_v) height, closestxy_d, closest_height2, movez_r)
height += count * step_dist height += count * step_dist
if decel_down_d > 0.: if decel_down_d > 0.:
count = mcu_stepper.step_delta_accel( count = mcu_stepper.step_delta_accel(
mcu_time + decel_time_offset, closest_d - decel_down_d, mcu_time + decel_time_offset, decel_down_d,
-step_dist, closest_d - decel_offset, -decel_offset, -accel_multiplier, -step_dist,
closest_height2, height, movez_r, -accel_multiplier) height, closestxy_d, closest_height2, movez_r)
###################################################################### ######################################################################

20
klippy/mcu.py
View File

@ -86,21 +86,25 @@ class MCU_stepper:
self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq)
self.commanded_position += count self.commanded_position += count
return count return count
def step_delta_const(self, mcu_time, dist, step_dist, start_pos def step_delta_const(self, mcu_time, dist, start_pos
, closest_height2, height, movez_r, inv_velocity): , inv_velocity, step_dist
, height, closestxy_d, closest_height2, movez_r):
clock = mcu_time * self._mcu_freq clock = mcu_time * self._mcu_freq
count = self.ffi_lib.stepcompress_push_delta_const( count = self.ffi_lib.stepcompress_push_delta_const(
self._stepqueue, clock, dist, step_dist, start_pos self._stepqueue, clock, dist, start_pos
, closest_height2, height, movez_r, inv_velocity * self._mcu_freq) , inv_velocity * self._mcu_freq, step_dist
, height, closestxy_d, closest_height2, movez_r)
self.commanded_position += count self.commanded_position += count
return count return count
def step_delta_accel(self, mcu_time, dist, step_dist, start_pos def step_delta_accel(self, mcu_time, dist, start_pos
, closest_height2, height, movez_r, accel_multiplier): , accel_multiplier, step_dist
, height, closestxy_d, closest_height2, movez_r):
clock = mcu_time * self._mcu_freq clock = mcu_time * self._mcu_freq
mcu_freq2 = self._mcu_freq**2 mcu_freq2 = self._mcu_freq**2
count = self.ffi_lib.stepcompress_push_delta_accel( count = self.ffi_lib.stepcompress_push_delta_accel(
self._stepqueue, clock, dist, step_dist, start_pos self._stepqueue, clock, dist, start_pos
, closest_height2, height, movez_r, accel_multiplier * mcu_freq2) , accel_multiplier * mcu_freq2, step_dist
, height, closestxy_d, closest_height2, movez_r)
self.commanded_position += count self.commanded_position += count
return count return count
def get_errors(self): def get_errors(self):

40
klippy/stepcompress.c
View File

@ -442,14 +442,15 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
// Schedule 'count' number of steps using the delta kinematic const speed // Schedule 'count' number of steps using the delta kinematic const speed
int32_t int32_t
stepcompress_push_delta_const( stepcompress_push_delta_const(
struct stepcompress *sc, double clock_offset, double dist, double step_dist struct stepcompress *sc, double clock_offset, double dist, double start_pos
, double start_pos, double closest_height2, double height, double movez_r , double inv_velocity, double step_dist
, double inv_velocity) , double height, double closestxy_d, double closest_height2, double movez_r)
{ {
// Calculate number of steps to take // Calculate number of steps to take
double zdist = dist * movez_r; double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist) double reldist = closestxy_d - movexy_r*dist;
- height - zdist) / step_dist + .5; double end_height = safe_sqrt(closest_height2 - reldist*reldist);
int count = (end_height - height + movez_r*dist) / step_dist + .5;
if (count <= 0 || count > 1000000) { if (count <= 0 || count > 1000000) {
if (count) if (count)
errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f" errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f"
@ -462,11 +463,12 @@ stepcompress_push_delta_const(
// 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;
start_pos += movexy_r*closestxy_d;
height += .5 * step_dist; height += .5 * step_dist;
while (qn < end) { while (qn < end) {
double zh = height*movez_r; double relheight = movexy_r*height - movez_r*closestxy_d;
double v = safe_sqrt(closest_height2 - height*height + zh*zh); double v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + zh + (step_dist > 0. ? -v : v); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_velocity; *qn++ = clock_offset + pos * inv_velocity;
height += step_dist; height += step_dist;
} }
@ -477,14 +479,15 @@ stepcompress_push_delta_const(
// Schedule 'count' number of steps using delta kinematic acceleration // Schedule 'count' number of steps using delta kinematic acceleration
int32_t int32_t
stepcompress_push_delta_accel( stepcompress_push_delta_accel(
struct stepcompress *sc, double clock_offset, double dist, double step_dist struct stepcompress *sc, double clock_offset, double dist, double start_pos
, double start_pos, double closest_height2, double height, double movez_r , double accel_multiplier, double step_dist
, double accel_multiplier) , double height, double closestxy_d, double closest_height2, double movez_r)
{ {
// Calculate number of steps to take // Calculate number of steps to take
double zdist = dist * movez_r; double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist) double reldist = closestxy_d - movexy_r*dist;
- height - zdist) / step_dist + .5; double end_height = safe_sqrt(closest_height2 - reldist*reldist);
int count = (end_height - height + movez_r*dist) / step_dist + .5;
if (count <= 0 || count > 1000000) { if (count <= 0 || count > 1000000) {
if (count) if (count)
errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f" errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f"
@ -497,11 +500,12 @@ stepcompress_push_delta_accel(
// 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;
start_pos += movexy_r*closestxy_d;
height += .5 * step_dist; height += .5 * step_dist;
while (qn < end) { while (qn < end) {
double zh = height*movez_r; double relheight = movexy_r*height - movez_r*closestxy_d;
double v = safe_sqrt(closest_height2 - height*height + zh*zh); double v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + zh + (step_dist > 0. ? -v : v); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
v = safe_sqrt(pos * accel_multiplier); v = safe_sqrt(pos * accel_multiplier);
*qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v); *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
height += step_dist; height += step_dist;