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
, double steps, double step_offset
, double clock_offset, double sqrt_offset, double factor);
int32_t stepcompress_push_delta_const(
struct stepcompress *sc, double clock_offset, double dist
, double step_dist, double start_pos, double closest_height2
, double height, double movez_r, double inv_velocity);
int32_t stepcompress_push_delta_accel(
struct stepcompress *sc, double clock_offset, double dist
, double step_dist, double start_pos, double closest_height2
, double height, double movez_r, double accel_multiplier);
int32_t stepcompress_push_delta_const(struct stepcompress *sc
, double clock_offset, double dist, double start_pos
, double inv_velocity, double step_dist, double height
, double closestxy_d, double closest_height2, double movez_r);
int32_t stepcompress_push_delta_accel(struct stepcompress *sc
, double clock_offset, double dist, double start_pos
, double accel_multiplier, double step_dist, double height
, double closestxy_d, double closest_height2, double movez_r);
void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
void stepcompress_queue_msg(struct stepcompress *sc
, uint32_t *data, int len);

133
klippy/delta.py
View File

@ -125,72 +125,25 @@ class DeltaKinematics:
raise homing.EndstopMoveError(end_pos)
if move.axes_d[2]:
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):
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]:
self.move_z(move_time, move)
return
if not axes_d[2]:
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:
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]
@ -214,80 +167,76 @@ class DeltaKinematics:
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
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
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_down_d = cruise_down_d = decel_down_d = 0.
if reverse_d <= 0.:
if reversexy_d <= 0.:
accel_down_d = accel_d
cruise_down_d = cruise_end_d
decel_down_d = move_d
elif reverse_d >= move_d:
elif reversexy_d >= movexy_d:
accel_up_d = accel_d
cruise_up_d = cruise_end_d
decel_up_d = move_d
elif reverse_d < accel_d:
accel_up_d = reverse_d
elif reversexy_d < accel_d * movexy_r:
accel_up_d = reversexy_d * move_d * inv_movexy_d
accel_down_d = accel_d
cruise_down_d = cruise_end_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
cruise_up_d = reverse_d
cruise_up_d = reversexy_d * move_d * inv_movexy_d
cruise_down_d = cruise_end_d
decel_down_d = move_d
else:
accel_up_d = accel_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
# Generate steps
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_dist = self.steppers[i].step_dist
height = step_pos*step_dist - closestz
height = step_pos*step_dist - origz
if accel_up_d > 0.:
count = mcu_stepper.step_delta_accel(
mcu_time - accel_time_offset, closest_d - accel_up_d,
step_dist, closest_d + accel_offset,
closest_height2, height, movez_r, accel_multiplier)
mcu_time - accel_time_offset, accel_up_d,
accel_offset, accel_multiplier, step_dist,
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if cruise_up_d > 0.:
count = mcu_stepper.step_delta_const(
mcu_time + accel_t, closest_d - cruise_up_d,
step_dist, closest_d - accel_d,
closest_height2, height, movez_r, inv_cruise_v)
mcu_time + accel_t, cruise_up_d,
-accel_d, inv_cruise_v, step_dist,
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if decel_up_d > 0.:
count = mcu_stepper.step_delta_accel(
mcu_time + decel_time_offset, closest_d - decel_up_d,
step_dist, closest_d - decel_offset,
closest_height2, height, movez_r, -accel_multiplier)
mcu_time + decel_time_offset, decel_up_d,
-decel_offset, -accel_multiplier, step_dist,
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if accel_down_d > 0.:
count = mcu_stepper.step_delta_accel(
mcu_time - accel_time_offset, closest_d - accel_down_d,
-step_dist, closest_d + accel_offset,
closest_height2, height, movez_r, accel_multiplier)
mcu_time - accel_time_offset, accel_down_d,
accel_offset, accel_multiplier, -step_dist,
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if cruise_down_d > 0.:
count = mcu_stepper.step_delta_const(
mcu_time + accel_t, closest_d - cruise_down_d,
-step_dist, closest_d - accel_d,
closest_height2, height, movez_r, inv_cruise_v)
mcu_time + accel_t, cruise_down_d,
-accel_d, inv_cruise_v, -step_dist,
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if decel_down_d > 0.:
count = mcu_stepper.step_delta_accel(
mcu_time + decel_time_offset, closest_d - decel_down_d,
-step_dist, closest_d - decel_offset,
closest_height2, height, movez_r, -accel_multiplier)
mcu_time + decel_time_offset, decel_down_d,
-decel_offset, -accel_multiplier, -step_dist,
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.commanded_position += count
return count
def step_delta_const(self, mcu_time, dist, step_dist, start_pos
, closest_height2, height, movez_r, inv_velocity):
def step_delta_const(self, mcu_time, dist, start_pos
, inv_velocity, step_dist
, height, closestxy_d, closest_height2, movez_r):
clock = mcu_time * self._mcu_freq
count = self.ffi_lib.stepcompress_push_delta_const(
self._stepqueue, clock, dist, step_dist, start_pos
, closest_height2, height, movez_r, inv_velocity * self._mcu_freq)
self._stepqueue, clock, dist, start_pos
, inv_velocity * self._mcu_freq, step_dist
, height, closestxy_d, closest_height2, movez_r)
self.commanded_position += count
return count
def step_delta_accel(self, mcu_time, dist, step_dist, start_pos
, closest_height2, height, movez_r, accel_multiplier):
def step_delta_accel(self, mcu_time, dist, start_pos
, accel_multiplier, step_dist
, height, closestxy_d, closest_height2, movez_r):
clock = mcu_time * self._mcu_freq
mcu_freq2 = self._mcu_freq**2
count = self.ffi_lib.stepcompress_push_delta_accel(
self._stepqueue, clock, dist, step_dist, start_pos
, closest_height2, height, movez_r, accel_multiplier * mcu_freq2)
self._stepqueue, clock, dist, start_pos
, accel_multiplier * mcu_freq2, step_dist
, height, closestxy_d, closest_height2, movez_r)
self.commanded_position += count
return count
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
int32_t
stepcompress_push_delta_const(
struct stepcompress *sc, double clock_offset, double dist, double step_dist
, double start_pos, double closest_height2, double height, double movez_r
, double inv_velocity)
struct stepcompress *sc, double clock_offset, double dist, double start_pos
, double inv_velocity, double step_dist
, double height, double closestxy_d, double closest_height2, double movez_r)
{
// Calculate number of steps to take
double zdist = dist * movez_r;
int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist)
- height - zdist) / step_dist + .5;
double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
double reldist = closestxy_d - movexy_r*dist;
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)
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
uint64_t *qn = sc->queue_next, *end = &qn[count];
clock_offset += 0.5;
start_pos += movexy_r*closestxy_d;
height += .5 * step_dist;
while (qn < end) {
double zh = height*movez_r;
double v = safe_sqrt(closest_height2 - height*height + zh*zh);
double pos = start_pos + zh + (step_dist > 0. ? -v : v);
double relheight = movexy_r*height - movez_r*closestxy_d;
double v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_velocity;
height += step_dist;
}
@ -477,14 +479,15 @@ stepcompress_push_delta_const(
// Schedule 'count' number of steps using delta kinematic acceleration
int32_t
stepcompress_push_delta_accel(
struct stepcompress *sc, double clock_offset, double dist, double step_dist
, double start_pos, double closest_height2, double height, double movez_r
, double accel_multiplier)
struct stepcompress *sc, double clock_offset, double dist, double start_pos
, double accel_multiplier, double step_dist
, double height, double closestxy_d, double closest_height2, double movez_r)
{
// Calculate number of steps to take
double zdist = dist * movez_r;
int count = (safe_sqrt(closest_height2 - dist*dist + zdist*zdist)
- height - zdist) / step_dist + .5;
double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
double reldist = closestxy_d - movexy_r*dist;
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)
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
uint64_t *qn = sc->queue_next, *end = &qn[count];
clock_offset += 0.5;
start_pos += movexy_r*closestxy_d;
height += .5 * step_dist;
while (qn < end) {
double zh = height*movez_r;
double v = safe_sqrt(closest_height2 - height*height + zh*zh);
double pos = start_pos + zh + (step_dist > 0. ? -v : v);
double relheight = movexy_r*height - movez_r*closestxy_d;
double v = safe_sqrt(closest_height2 - 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);
height += step_dist;