From 9c932ad514cbf5d8ab9573b16aeceabb11ecfebf Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 4 Dec 2016 19:30:35 -0500 Subject: 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/delta.py | 133 +++++++++++++++++--------------------------------------- 1 file changed, 41 insertions(+), 92 deletions(-) (limited to 'klippy/delta.py') 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) ###################################################################### -- cgit v1.2.3-70-g09d2