From 9c932ad514cbf5d8ab9573b16aeceabb11ecfebf Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 4 Dec 2016 19:30:35 -0500 Subject: [PATCH] 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 --- klippy/chelper.py | 16 ++--- klippy/delta.py | 133 +++++++++++++----------------------------- klippy/mcu.py | 20 ++++--- klippy/stepcompress.c | 40 +++++++------ 4 files changed, 83 insertions(+), 126 deletions(-) diff --git a/klippy/chelper.py b/klippy/chelper.py index 58049582..4384fe28 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -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); diff --git a/klippy/delta.py b/klippy/delta.py index 19984496..7d7b063f 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -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) ###################################################################### diff --git a/klippy/mcu.py b/klippy/mcu.py index c1abb959..4c2bbc68 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -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): diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 4f41cfee..0af0672e 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -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;