From 51f943706bd5a771df7a30bd675b6a66a35214d9 Mon Sep 17 00:00:00 2001 From: Chris Lee Date: Wed, 6 Apr 2022 01:23:39 -0600 Subject: [PATCH] parent 64fff4eaf36f92ff0286502bc8ab615d6066c313 author Brandon Nance 1704668106 -0500 committer Brandon Nance 1704668106 -0500 parent 64fff4eaf36f92ff0286502bc8ab615d6066c313 author Brandon Nance 1704668106 -0500 committer Brandon Nance 1704668106 -0500 parent 64fff4eaf36f92ff0286502bc8ab615d6066c313 author Brandon Nance 1704668106 -0500 committer Brandon Nance 1704668106 -0500 add PolarXZ kinematics --- config/example-polarxz.cfg | 74 ++ klippy/chelper/__init__.py | 14 + klippy/chelper/itersolve.c | 1 + klippy/chelper/kin_polarbed.c | 34 + klippy/chelper/kin_polarxz.c | 76 ++ klippy/configfile.py | 1 + klippy/extras/bed_mesh.py | 284 +++++- klippy/extras/force_move.py | 195 +++- klippy/extras/homeable_stepper.py | 325 +++++++ klippy/extras/homing.py | 17 + klippy/extras/manual_probe.py | 1 + klippy/extras/polar_alignment.py | 132 +++ klippy/extras/probe.py | 2 + klippy/extras/screws_tilt_adjust.py | 41 +- klippy/kinematics/polarxz.py | 1280 +++++++++++++++++++++++++++ klippy/stepper.py | 7 +- klippy/toolhead.py | 74 +- klippy/util.py | 2 + scripts/klipper_git_reset.sh | 0 test/klippy/polarxz.test | 74 ++ 20 files changed, 2591 insertions(+), 43 deletions(-) create mode 100644 config/example-polarxz.cfg create mode 100644 klippy/chelper/kin_polarbed.c create mode 100644 klippy/chelper/kin_polarxz.c create mode 100644 klippy/extras/homeable_stepper.py create mode 100644 klippy/extras/polar_alignment.py create mode 100644 klippy/kinematics/polarxz.py create mode 100644 scripts/klipper_git_reset.sh create mode 100644 test/klippy/polarxz.test diff --git a/config/example-polarxz.cfg b/config/example-polarxz.cfg new file mode 100644 index 000000000..1c25a260a --- /dev/null +++ b/config/example-polarxz.cfg @@ -0,0 +1,74 @@ +# This file is an example config file for polar style printers. One +# may copy and edit this file to configure a new polar printer. + +# POLAR KINEMATICS ARE A WORK IN PROGRESS. Moves around the 0, 0 +# position are known to not work properly. + +# See docs/Config_Reference.md for a description of parameters. + +[stepper_bed] +step_pin: PF0 +dir_pin: PF1 +enable_pin: !PD7 +microsteps: 16 +gear_ratio: 80:16 + +[stepper_x] +step_pin: PF6 +dir_pin: PF7 +enable_pin: !PF2 +microsteps: 16 +rotation_distance: 40 +endstop_pin: ^PJ1 +position_endstop: 300 +position_max: 300 +homing_speed: 50 + +[stepper_z] +step_pin: PL3 +dir_pin: PL1 +enable_pin: !PK0 +microsteps: 16 +rotation_distance: 8 +endstop_pin: ^PD3 +position_endstop: 0.5 +position_max: 200 + +[extruder] +step_pin: PA4 +dir_pin: PA6 +enable_pin: !PA2 +microsteps: 16 +rotation_distance: 33.500 +nozzle_diameter: 0.400 +filament_diameter: 1.750 +heater_pin: PB4 +sensor_type: ATC Semitec 104GT-2 +sensor_pin: PK5 +control: pid +pid_Kp: 22.2 +pid_Ki: 1.08 +pid_Kd: 114 +min_temp: 0 +max_temp: 250 + +[heater_bed] +heater_pin: PH5 +sensor_type: EPCOS 100K B57560G104F +sensor_pin: PK6 +control: watermark +min_temp: 0 +max_temp: 130 + +[fan] +pin: PH6 + +[mcu] +serial: /dev/ttyACM0 + +[printer] +kinematics: polarxz +max_velocity: 300 +max_accel: 3000 +max_z_velocity: 25 +max_z_accel: 30 diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 608e62d1a..d545f1c34 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -33,6 +33,8 @@ "kin_delta.c", "kin_deltesian.c", "kin_polar.c", + "kin_polarxz.c", + "kin_polarbed.c", "kin_rotary_delta.c", "kin_winch.c", "kin_extruder.c", @@ -149,6 +151,14 @@ struct stepper_kinematics *polar_stepper_alloc(char type); """ +defs_kin_polarxz = """ + struct stepper_kinematics *polarxz_stepper_alloc(char type); +""" + +defs_kin_polarbed = """ + struct stepper_kinematics *polarbed_stepper_alloc(char type); +""" + defs_kin_rotary_delta = """ struct stepper_kinematics *rotary_delta_stepper_alloc( double shoulder_radius, double shoulder_height @@ -251,6 +261,8 @@ defs_kin_delta, defs_kin_deltesian, defs_kin_polar, + defs_kin_polarxz, + defs_kin_polarbed, defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder, @@ -258,6 +270,7 @@ defs_kin_idex, ] + # Update filenames to an absolute path def get_abs_files(srcdir, filelist): return [os.path.join(srcdir, fname) for fname in filelist] @@ -305,6 +318,7 @@ def do_build_code(cmd): FFI_lib = None pyhelper_logging_callback = None + # Hepler invoked from C errorf() code to log errors def logging_callback(msg): logging.error(FFI_main.string(msg)) diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 0dbc6c512..7d8e59edb 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -104,6 +104,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m continue; } } + // Found next step - submit it int ret = stepcompress_append(sk->sc, sdir, m->print_time, guess.time); if (ret) diff --git a/klippy/chelper/kin_polarbed.c b/klippy/chelper/kin_polarbed.c new file mode 100644 index 000000000..d69597559 --- /dev/null +++ b/klippy/chelper/kin_polarbed.c @@ -0,0 +1,34 @@ +#include // sqrt +#include // malloc +#include // memset +#include "compiler.h" // __visible +#include "itersolve.h" // struct stepper_kinematics +#include "trapq.h" // move_get_coord +#include "pyhelper.h" // errorf + +static double +polarbed_stepper_angle_calc_position(struct stepper_kinematics *sk, struct move *m, double move_time) + +{ + struct coord c = move_get_coord(m, move_time); + // XXX - handle x==y==0 + if (c.x == 0 && c.y == 0) + errorf("polarbed_stepper_angle_calc_position: x==y==0"); + + double angle = atan2(c.y, c.x); + + errorf("polarbed_stepper_angle_calc_position: x=%f y=%f angle=%f", c.x, c.y, angle * 180.0 / 3.14159); + + return angle; +} + +struct stepper_kinematics *__visible +polarbed_stepper_alloc(char type) +{ + struct stepper_kinematics *sk = malloc(sizeof(*sk)); + memset(sk, 0, sizeof(*sk)); + sk->calc_position_cb = polarbed_stepper_angle_calc_position; + sk->active_flags = AF_X | AF_Y; + + return sk; +} diff --git a/klippy/chelper/kin_polarxz.c b/klippy/chelper/kin_polarxz.c new file mode 100644 index 000000000..6f3be1381 --- /dev/null +++ b/klippy/chelper/kin_polarxz.c @@ -0,0 +1,76 @@ +#include // sqrt +#include // malloc +#include // memset +#include "compiler.h" // __visible +#include "itersolve.h" // struct stepper_kinematics +#include "trapq.h" // move_get_coord + +static double +polarxz_stepper_angle_calc_position(struct stepper_kinematics *sk, struct move *m, double move_time) +{ + struct coord c = move_get_coord(m, move_time); + // XXX - handle x==y==0 + if (c.x == 0 && c.y == 0) + return 0; + double angle = atan2(c.y, c.x); + if (angle - sk->commanded_pos > M_PI) + angle -= 2.f * M_PI; + else if (angle - sk->commanded_pos < -M_PI) + angle += 2.f * M_PI; + angle = round(angle * 1000000) / 1000000; + + return angle; +} + +static void +polarxz_stepper_angle_post_fixup(struct stepper_kinematics *sk) +{ + // Normalize the stepper_bed angle + if (sk->commanded_pos < -M_PI) + sk->commanded_pos += 2 * M_PI; + else if (sk->commanded_pos > M_PI) + sk->commanded_pos -= 2 * M_PI; +} + +static double +polarxz_stepper_plus_calc_position(struct stepper_kinematics *sk, struct move *m, double move_time) +{ + struct coord c = move_get_coord(m, move_time); + float pos = (sqrt(c.x * c.x + c.y * c.y)) + c.z; + pos = round(pos * 1000000) / 1000000; + return pos; +} + +static double +polarxz_stepper_minus_calc_position(struct stepper_kinematics *sk, struct move *m, double move_time) +{ + struct coord c = move_get_coord(m, move_time); + float pos = (sqrt(c.x * c.x + c.y * c.y)) - c.z; + pos = round(pos * 1000000) / 1000000; + return pos; +} + +struct stepper_kinematics *__visible +polarxz_stepper_alloc(char type) +{ + struct stepper_kinematics *sk = malloc(sizeof(*sk)); + memset(sk, 0, sizeof(*sk)); + if (type == '+') + { + sk->calc_position_cb = polarxz_stepper_plus_calc_position; + sk->active_flags = AF_X | AF_Y | AF_Z; + } + else if (type == '-') + { + sk->calc_position_cb = polarxz_stepper_minus_calc_position; + sk->active_flags = AF_X | AF_Y | AF_Z; + } + else if (type == 'a') + { + sk->calc_position_cb = polarxz_stepper_angle_calc_position; + sk->post_cb = polarxz_stepper_angle_post_fixup; + sk->active_flags = AF_X | AF_Y; + } + + return sk; +} diff --git a/klippy/configfile.py b/klippy/configfile.py index 385541740..d440251f3 100644 --- a/klippy/configfile.py +++ b/klippy/configfile.py @@ -434,6 +434,7 @@ def read_main_config(self): def check_unused_options(self, config, error_on_unused): fileconfig = config.fileconfig objects = dict(self.printer.lookup_objects()) + logging.info("objects: %s", objects) # Determine all the fields that have been accessed access_tracking = dict(config.access_tracking) for section in self.autosave.fileconfig.sections(): diff --git a/klippy/extras/bed_mesh.py b/klippy/extras/bed_mesh.py index f67786fff..e012c4e22 100644 --- a/klippy/extras/bed_mesh.py +++ b/klippy/extras/bed_mesh.py @@ -26,6 +26,19 @@ class BedMeshError(Exception): pass +def linear_interpolate(start, stop, n): + """ + Linearly interpolate from start to stop over n steps. + """ + interp_values = [] + if n == 1: + return [(start + stop) / 2] + for i in range(n): + interp = start + (stop - start) * (i / (n - 1)) + interp_values.append(interp) + return interp_values + + # PEP 485 isclose() def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): return abs(a - b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol) @@ -411,25 +424,58 @@ def _generate_points(self, error): max_x = min_x + x_dist * (x_cnt - 1) pos_y = min_y points = [] - for i in range(y_cnt): - for j in range(x_cnt): - if not i % 2: - # move in positive directon - pos_x = min_x + j * x_dist - else: - # move in negative direction - pos_x = max_x - j * x_dist - if self.radius is None: - # rectangular bed, append - points.append((pos_x, pos_y)) - else: - # round bed, check distance from origin - dist_from_origin = math.sqrt(pos_x * pos_x + pos_y * pos_y) - if dist_from_origin <= self.radius: - points.append( - (self.origin[0] + pos_x, self.origin[1] + pos_y) + + if self.polar == True: + radius_points = x_cnt // 2 + radius = self.radius + spacing = radius // radius_points + radius_steps = [] + for i in range(radius_points): + radius_steps.append(radius) + radius -= spacing + + num_points_per_quadrant = radius_points + polar_points = [] + for radius_step in radius_steps: + total_points = num_points_per_quadrant * 4 + angle = 0 + for i in range(total_points): + polar_point = (radius_step, angle) + polar_points.append(polar_point) + angle += (math.pi / 2) / num_points_per_quadrant + if angle > (math.pi * 2): + angle = angle - (math.pi * 2) + num_points_per_quadrant -= 1 + polar_points.append((0, 0)) + for polar_point in polar_points: + radius = polar_point[0] + angle = polar_point[1] + x = radius * math.cos(angle) + y = radius * math.sin(angle) + points.append((x, y)) + else: + # rectangular bed logic + for i in range(y_cnt): + for j in range(x_cnt): + if not i % 2: + # move in positive directon + pos_x = min_x + j * x_dist + else: + # move in negative direction + pos_x = max_x - j * x_dist + if self.radius is None: + # rectangular bed, append + points.append((pos_x, pos_y)) + else: + # round bed, check distance from origin + dist_from_origin = math.sqrt( + pos_x * pos_x + pos_y * pos_y ) - pos_y += y_dist + if dist_from_origin <= self.radius: + points.append( + (self.origin[0] + pos_x, self.origin[1] + pos_y) + ) + pos_y += y_dist self.points = points rri = self.relative_reference_index if self.zero_ref_pos is None and rri is not None: @@ -546,6 +592,7 @@ def _init_mesh_config(self, config): mesh_cfg = self.mesh_config orig_cfg = self.orig_config self.radius = config.getfloat("mesh_radius", None, above=0.0) + self.polar = config.getboolean("polar", False) if self.radius is not None: self.origin = config.getfloatlist( "mesh_origin", (0.0, 0.0), count=2 @@ -772,6 +819,60 @@ def probe_finalize(self, offsets, positions): % (ref_pos[0], ref_pos[1], ref_pos[2]) ) z_offset = ref_pos[2] + kin = self.bedmesh.toolhead.get_kinematics() + if self.polar: + # polar! need to re-order points + x_cnt = self.mesh_config["x_count"] + y_cnt = self.mesh_config["y_count"] + min_x, min_y = self.mesh_min + max_x, max_y = self.mesh_max + x_dist = (max_x - min_x) / (x_cnt - 1) + y_dist = (max_y - min_y) / (y_cnt - 1) + # floor distances down to next hundredth + x_dist = math.floor(x_dist * 100) / 100 + y_dist = math.floor(y_dist * 100) / 100 + pos_y = min_y + new_positions = [] + expected_points = [] + # build list of expected points + + for i in range(y_cnt): + for j in range(x_cnt): + if not i % 2: + # move in positive directon + pos_x = min_x + j * x_dist + else: + # move in negative direction + pos_x = max_x - j * x_dist + # round bed, check distance from origin + dist_from_origin = math.sqrt(pos_x * pos_x + pos_y * pos_y) + if dist_from_origin <= self.radius: + expected_points.append( + (self.origin[0] + pos_x, self.origin[1] + pos_y) + ) + pos_y += y_dist + # reorder the points to be from bottom up like klipper wants + # find next_point in positions + for expected_point in expected_points: + for p in positions: + if isclose(p[0], expected_point[0]) and isclose( + p[1], expected_point[1] + ): + new_positions.append(p) + break + elif expected_point[0] == 0 and expected_point[1] == 0: + zero_cross_rounded = round(kin.zero_crossing_radius, 2) + if ( + abs(p[0]) <= zero_cross_rounded + and abs(p[1]) <= zero_cross_rounded + ): + p[0] = 0 + p[1] = 0 + new_positions.append(p) + logging.info("expected_points: %s", expected_points) + logging.info("original_positions: %s", positions) + logging.info("new_positions: %s", new_positions) + positions = new_positions params = dict(self.mesh_config) params["min_x"] = min(positions, key=lambda p: p[0])[0] + x_offset params["max_x"] = max(positions, key=lambda p: p[0])[0] + x_offset @@ -827,6 +928,11 @@ def probe_finalize(self, offsets, positions): ) positions = corrected_pts + if self.relative_reference_index is not None: + # zero out probe z offset and + # set offset relative to reference index + z_offset = positions[self.relative_reference_index][2] + probed_matrix = [] row = [] prev_pos = positions[0] @@ -855,8 +961,146 @@ def probe_finalize(self, offsets, positions): % (len(probed_matrix), str(probed_matrix)) ) - if self.radius is not None: - # round bed, extrapolate probed values to create a square mesh + if self.polar: + """ + interpolate each "ring" of numbers in the matrix + """ + x_cnt = len(probed_matrix) + for row in probed_matrix: + row_size = len(row) + if not row_size & 1: + # an even number of points in a row shouldn't be possible + msg = "bed_mesh: incorrect number of points sampled on X\n" + msg += "Probed Table:\n" + msg += str(probed_matrix) + raise self.gcode.error(msg) + buf_cnt = (x_cnt - row_size) // 2 + if buf_cnt == 0: + continue + left_buffer = [0] * buf_cnt + right_buffer = [0] * buf_cnt + row[0:0] = left_buffer + row.extend(right_buffer) + logging.info( + "bed_mesh: zero-padded matrix: %s" % str(probed_matrix) + ) + n = x_cnt + center = n // 2 + # i'm sorry. + for r in range(center, 0, -1): + row_depth = center - r + mid_top = probed_matrix[row_depth][center] + mid_bottom = probed_matrix[n - row_depth - 1][center] + mid_left = probed_matrix[center][row_depth] + mid_right = probed_matrix[center][n - row_depth - 1] + # fill in top left corner interpolation + probed_matrix[row_depth][row_depth] = (mid_top + mid_left) / 2 + # fill in top right corner interpolation + probed_matrix[row_depth][n - row_depth - 1] = ( + mid_top + mid_right + ) / 2 + # fill in bottom left corner interpolation + probed_matrix[n - row_depth - 1][row_depth] = ( + mid_bottom + mid_left + ) / 2 + # fill in bottom right corner interpolation + probed_matrix[n - row_depth - 1][n - row_depth - 1] = ( + mid_bottom + mid_right + ) / 2 + + interpolations_left_in_each_row = (r * 2) - 2 + left_in_each_side = interpolations_left_in_each_row // 2 + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[row_depth][row_depth] + stop_val = mid_top + left_top_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in left side of top row of probed_matrix with left_top_vals + for i in range(left_in_each_side): + probed_matrix[row_depth][row_depth + i + 1] = left_top_vals[ + i + ] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[row_depth][n - row_depth - 1] + stop_val = mid_top + right_top_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in right side of top row of probed_matrix with right_top_vals + for i in range(left_in_each_side): + probed_matrix[row_depth][ + n - row_depth - 1 - i - 1 + ] = right_top_vals[i] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[n - row_depth - 1][row_depth] + stop_val = mid_bottom + left_bottom_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in left side of bottom row of probed_matrix with left_bottom_vals + for i in range(left_in_each_side): + probed_matrix[n - row_depth - 1][ + row_depth + i + 1 + ] = left_bottom_vals[i] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[n - row_depth - 1][n - row_depth - 1] + stop_val = mid_bottom + right_bottom_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in right side of bottom row of probed_matrix with right_bottom_vals + for i in range(left_in_each_side): + probed_matrix[n - row_depth - 1][ + n - row_depth - 1 - i - 1 + ] = right_bottom_vals[i] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[row_depth][row_depth] + stop_val = mid_left + top_left_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in top side of left row of probed_matrix with top_left_vals + for i in range(left_in_each_side): + probed_matrix[row_depth + i + 1][row_depth] = top_left_vals[ + i + ] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[row_depth][n - row_depth - 1] + stop_val = mid_right + top_right_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in top side of right row of probed_matrix with top_right_vals + for i in range(left_in_each_side): + probed_matrix[row_depth + i + 1][ + n - row_depth - 1 + ] = top_right_vals[i] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[n - row_depth - 1][row_depth] + stop_val = mid_left + bottom_left_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in bottom side of left row of probed_matrix with bottom_left_vals + for i in range(left_in_each_side): + probed_matrix[n - row_depth - 1 - i - 1][ + row_depth + ] = bottom_left_vals[i] + # interpolate from corner to mid value over the length of left_in_each_side + start_val = probed_matrix[n - row_depth - 1][n - row_depth - 1] + stop_val = mid_right + bottom_right_vals = linear_interpolate( + start_val, stop_val, left_in_each_side + ) + # fill in bottom side of right row of probed_matrix with bottom_right_vals + for i in range(left_in_each_side): + probed_matrix[n - row_depth - 1 - i - 1][ + n - row_depth - 1 + ] = bottom_right_vals[i] + logging.info("probed_matrix: %s", probed_matrix) + elif self.radius is not None: + # nonpolar round bed, extrapolate probed values to create a square mesh for row in probed_matrix: row_size = len(row) if not row_size & 1: diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index f55c291ad..f4cd4bb1a 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -12,6 +12,7 @@ BUZZ_RADIANS_VELOCITY = BUZZ_RADIANS_DISTANCE / 0.250 STALL_TIME = 0.100 + # Calculate a move's accel_t, cruise_t, and cruise_v def calc_move_time(dist, speed, accel): axis_r = 1.0 @@ -29,6 +30,187 @@ def calc_move_time(dist, speed, accel): return axis_r, accel_t, cruise_t, speed +def calc_move_time_polar(dist, speed, accel): + # dist in degs, speed in deg/s and accel in deg/s/s + # except i think the math is the same no matter what, so extra func is not needed + axis_r = 1.0 + if dist < 0.0: + axis_r = -1.0 + dist = -dist + if not accel or not dist: + return axis_r, 0.0, dist / speed, speed + max_cruise_v2 = dist * accel + if max_cruise_v2 < speed**2: + speed = math.sqrt(max_cruise_v2) + accel_t = speed / accel + accel_decel_d = accel_t * speed + cruise_t = (dist - accel_decel_d) / speed + return axis_r, accel_t, cruise_t, speed + + +def distance(p1, p2): + return math.sqrt(((p2[0] - p1[0]) ** 2) + ((p2[1] - p1[1]) ** 2)) + + +def cartesian_to_polar(x, y): + return (math.sqrt(x**2 + y**2), math.atan2(y, x)) + + +def polar_to_cartesian(r, theta): + return (r * math.cos(theta), r * math.sin(theta)) + + +def calc_move_time_polar(angle, speed, accel): + # angle in degs, speed in deg/s and accel in deg/s/s + # same as calc_move_time_polar, but axis_r (normalized move vector) needs to match such that + # only the bed moves the given distance + RADIUS = 10 + if not angle: + angle = 0 + if accel == 0: + accel = 10 + segmentation_angle_degs = 90 + num_segments = int(angle / float(segmentation_angle_degs)) + if angle % segmentation_angle_degs != 0: + num_segments += 1 + cartesian_start = (RADIUS, 0) + max_cruise_v2 = angle * accel + moves = [] + if max_cruise_v2 < speed**2: + speed = math.sqrt(max_cruise_v2) + cur_speed = 0 + prev_speed = 0 + state = "accelerating" + prev_cartesian_velocity = 0 + for i in range(num_segments): + is_last = i == num_segments - 1 + start_angle_degs = i * segmentation_angle_degs + end_angle_degs = (i + 1) * segmentation_angle_degs + if end_angle_degs > angle: + end_angle_degs = angle + start_angle = math.radians(start_angle_degs) + ending_angle = math.radians(end_angle_degs) + angle_delta = ending_angle - start_angle + cartesian_end = polar_to_cartesian(RADIUS, ending_angle) + cartesian_end = ( + round(cartesian_end[0], 10), + round(cartesian_end[1], 10), + ) + x_move = cartesian_end[0] - cartesian_start[0] + y_move = cartesian_end[1] - cartesian_start[1] + total_move_dist = math.sqrt(x_move**2 + y_move**2) + inv_dist = 1.0 / total_move_dist + x_ratio = x_move * inv_dist + y_ratio = y_move * inv_dist + # x_ratio = round(abs(x_move) / (abs(x_move) + abs(y_move)), 10) + # y_ratio = round(abs(y_move) / (abs(x_move) + abs(y_move)), 10) + # if x_move < 0: + # x_ratio = -x_ratio + # if y_move < 0: + # y_ratio = -y_ratio + print("moving from %s to %s" % (cartesian_start, cartesian_end)) + # how long it takes to get up to cruising speed + angle_delta_degs = math.degrees(angle_delta) + decel_t = 0 + accel_t = 0 + accel_d = 0 + decel_d = 0 + prev_speed = cur_speed + if state == "cruising": + decel_t = cur_speed / accel + speed_left = 0 - cur_speed + decel_d = ( + decel_t * speed_left + ) # how far we have to spin to get up to speed + if is_last: + state = "decelerating" + else: + speed_left = speed - cur_speed + accel_t = speed_left / accel + accel_d = ( + accel_t * speed_left + ) # how far we have to spin to get up to speed + if accel_d > angle_delta_degs: + # if we won't get up to speed before we hit the end of the move + if state == "cruising": + state = "decelerating" + cruise_t = 0 # we won't be cruising at all + accel_t = math.sqrt(angle_delta_degs / accel) + cur_speed = cur_speed + (accel * accel_t) # add acceled speed + + elif ( + abs(decel_d) > angle_delta_degs + ): # if we can't stop entirely this move + if state == "cruising": + state = "decelerating" + cruise_t = 0 # we won't be cruising at all + decel_t = math.sqrt(angle_delta_degs / accel) + cur_speed = cur_speed - (accel * accel_t) # substract acceled speed + else: + if state == "accelerating": + state = "cruising" + cruise_t = (total_move_dist - abs(accel_d)) / speed + elif state == "cruising": + cruise_t = (total_move_dist) / speed + elif state == "decelerating": + cruise_t = (total_move_dist - abs(decel_d)) / speed + cur_speed = speed + if num_segments == 1: + decel_t = accel_t + cruise_t -= decel_t + elif state != "decelerating": + decel_t = 0 + + l = 0.5 * total_move_dist + sagitta = RADIUS - math.sqrt(RADIUS**2 - l**2) + radius_arm_traveled_dist = sagitta * 2 + total_move_time = accel_t + cruise_t + decel_t + radius_arm_velocity = radius_arm_traveled_dist / total_move_time + radius_arm_accel = radius_arm_velocity / accel_t + # x_velocity = r_velocity * cos(theta) - r_theta_velocity * sin(theta) + # y_velocity = r_velocity * sin(theta) + r_theta_velocity * cos(theta) + + # x_accel = r_accel * cos(theta) - r_theta_accel * sin(theta) + + # x_accel = radius_arm_accel * math.cos(angle_delta) - r_theta_accel * math.sin(angle_delta) - r_theta_velocity**2 * math.cos(angle_delta) + # x_accel = -r_theta_accel * sin(theta) - r_theta_velocity^2 * cos(theta) + # y_accel = +r_theta_accel * cos(theta) - r_theta_velocity^2 * sin(theta) + + # x_acceleration = (dr_velocity/dt) * cos(angle_delta) - r_velocity * sin(angle_delta) * (dθ/dt) - (dr_theta_velocity/dt) * sin(angle_delta) - r_theta_velocity^2 * cos(theta) + # y_acceleration = (dr_velocity/dt) * sin(angle_delta) + r_velocity * cos(angle_delta) * (dθ/dt) + (dr_theta_velocity/dt) * cos(angle_delta) - r_theta_velocity^2 * sin(theta) + + x_velocity = -speed * math.sin(angle_delta) + y_velocity = speed * math.cos(angle_delta) + total_velocity = math.sqrt(x_velocity**2 + y_velocity**2) + + x_accel = -accel * math.sin(angle_delta) - speed**2 * math.cos( + angle_delta + ) + y_accel = accel * math.cos(angle_delta) - speed**2 * math.sin( + angle_delta + ) + total_accel = math.sqrt(x_accel**2 + y_accel**2) + + move = ( + cartesian_end[0], + cartesian_end[1], + x_ratio, + y_ratio, + round(accel_t, 10), + round(cruise_t, 10), + round(decel_t, 10), + total_velocity, + prev_cartesian_velocity, + total_accel, + ) + prev_cartesian_velocity = total_velocity + moves.append(move) + print(num_segments) + cartesian_start = cartesian_end + + return moves + + class ForceMove: def __init__(self, config): self.printer = config.get_printer() @@ -41,6 +223,9 @@ def __init__(self, config): self.stepper_kinematics = ffi_main.gc( ffi_lib.cartesian_stepper_alloc(b"x"), ffi_lib.free ) + self.polar_bed_stepper_kinematics = ffi_main.gc( + ffi_lib.polarbed_stepper_alloc(b"a"), ffi_lib.free + ) # Register commands gcode = self.printer.lookup_object("gcode") gcode.register_command( @@ -89,6 +274,12 @@ def _restore_enable(self, stepper, was_enable): def manual_move(self, stepper, dist, speed, accel=0.0): toolhead = self.printer.lookup_object("toolhead") + if stepper.units_in_radians: + # convert convert radians to degrees + dist = math.radians(dist) + speed = math.radians(speed) + accel = math.radians(accel) + toolhead.flush_step_generation() prev_sk = stepper.set_stepper_kinematics(self.stepper_kinematics) prev_trapq = stepper.set_trapq(self.trapq) @@ -114,10 +305,10 @@ def manual_move(self, stepper, dist, speed, accel=0.0): print_time = print_time + accel_t + cruise_t + accel_t stepper.generate_steps(print_time) self.trapq_finalize_moves(self.trapq, print_time + 99999.9) - stepper.set_trapq(prev_trapq) - stepper.set_stepper_kinematics(prev_sk) toolhead.note_kinematic_activity(print_time) toolhead.dwell(accel_t + cruise_t + accel_t) + stepper.set_trapq(prev_trapq) + stepper.set_stepper_kinematics(prev_sk) def _lookup_stepper(self, gcmd): name = gcmd.get("STEPPER") diff --git a/klippy/extras/homeable_stepper.py b/klippy/extras/homeable_stepper.py new file mode 100644 index 000000000..b4e4948d0 --- /dev/null +++ b/klippy/extras/homeable_stepper.py @@ -0,0 +1,325 @@ +# Support for a homeable stepper +# +# Copyright (C) 2019-2021 Kevin O'Connor +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import math +import chelper +import logging +from . import force_move +from stepper import MCU_stepper +from stepper import parse_step_distance +from toolhead import DripModeEndSignal, DRIP_SEGMENT_TIME, DRIP_TIME, MOVE_BATCH_TIME, SDS_CHECK_TIME + +class HomeableStepper(MCU_stepper): + def __init__(self, config, *args, **kwargs): + MCU_stepper.__init__(self, *args, **kwargs) + # super(HomeableStepper, self).__init__(*args, **kwargs) + self.printer = config.get_printer() + self.can_home = True + self.next_cmd_time = 0.0 + # Setup iterative solver + ffi_main, ffi_lib = chelper.get_ffi() + self.homing_trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) + self.trapq_append = ffi_lib.trapq_append + self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves + self.reactor = self.printer.get_reactor() + self.all_mcus = [ + m for n, m in self.printer.lookup_objects(module='mcu')] + self.mcu = self.all_mcus[0] + # Register commands + gcode = self.printer.lookup_object("gcode") + self.endstops = [] + self.endstop_map = {} + self.add_endstop(config) + + def setup_itersolve_for_homing(self): + self.setup_itersolve("cartesian_stepper_alloc", b"x") + self.set_trapq(self.homing_trapq) + + def sync_print_time(self, force_update=False): + toolhead = self.printer.lookup_object("toolhead") + print_time = toolhead.get_last_move_time() + logging.info('syncing print time...') + logging.info("print_time: %s", print_time) + logging.info("next_cmd_time: %s", self.next_cmd_time) + if self.next_cmd_time > print_time and not force_update: + logging.info("dwelling for %s", self.next_cmd_time - print_time) + toolhead.dwell(self.next_cmd_time - print_time) + else: + logging.info("not dwelling") + self.next_cmd_time = print_time + logging.info("done syncing print time!") + + def do_enable(self, enable): + logging.info("enabling...") + self.sync_print_time() + stepper_enable = self.printer.lookup_object("stepper_enable") + if enable: + se = stepper_enable.lookup_enable(self.get_name()) + se.motor_enable(self.next_cmd_time) + else: + se = stepper_enable.lookup_enable(self.get_name()) + se.motor_disable(self.next_cmd_time) + self.sync_print_time() + logging.info("done enabling...") + + def do_set_position(self, setpos): + self.set_position([setpos, 0.0, 0.0]) + + def do_manual_move(self, movepos, speed, accel, sync=True, drip_completion=None): + logging.info("start of do_manual_move") + self.sync_print_time() + cp = self.get_commanded_position() + + dist = movepos - cp + logging.info("dist: %s", dist) + axis_r, accel_t, cruise_t, cruise_v = force_move.calc_move_time( + dist, speed, accel + ) + self.trapq_append( + self._trapq, + self.next_cmd_time, + accel_t, + cruise_t, + accel_t, + cp, + 0.0, + 0.0, + axis_r, + 0.0, + 0.0, + 0.0, + cruise_v, + accel, + ) + self.next_cmd_time = self.next_cmd_time + accel_t + cruise_t + accel_t + if drip_completion is not None: + logging.info("drip start...") + next_cmd_time = self.next_cmd_time + toolhead = self.printer.lookup_object("toolhead") + print_time = toolhead.get_last_move_time() + logging.info("dripping, next cmd time: %s, print time: %s", next_cmd_time, print_time) + move_flush_time = toolhead.move_flush_time + kin_flush_delay = SDS_CHECK_TIME + flush_delay = DRIP_TIME + move_flush_time + kin_flush_delay + raise_drip_signal = False + while print_time < next_cmd_time: + if drip_completion.test(): + raise_drip_signal = True + break + curtime = self.reactor.monotonic() + est_print_time = self.mcu.estimated_print_time(curtime) + wait_time = print_time - est_print_time - flush_delay + if wait_time > 0. and toolhead.can_pause: + # Pause before sending more steps + drip_completion.wait(curtime + wait_time) + continue + npt = min(print_time + DRIP_SEGMENT_TIME, next_cmd_time) + sg_flush_time = npt - kin_flush_delay + logging.info("drip generating steps, sg_flush_time: %s", sg_flush_time) + self.generate_steps(sg_flush_time) + free_time = sg_flush_time - kin_flush_delay + self.trapq_finalize_moves(self._trapq, free_time) + mcu_flush_time = sg_flush_time - move_flush_time + for m in self.all_mcus: + m.flush_moves(mcu_flush_time) + print_time = npt + + # toolhead.note_kinematic_activity(print_time) + self.sync_print_time(force_update=True) + logging.info("dripping done.") + if raise_drip_signal: + raise DripModeEndSignal() + + + else: + self.generate_steps(self.next_cmd_time) + self.trapq_finalize_moves(self._trapq, self.next_cmd_time + 99999.9) + toolhead = self.printer.lookup_object("toolhead") + toolhead.note_kinematic_activity(self.next_cmd_time) + if sync: + logging.info("end of do_manual_move") + self.sync_print_time() + + def home(self, accel): + #homing dist is one full rotation + self.homing_accel = accel + logging.info("accel: %s", accel) + homing_dist = 3*math.pi + if self.homing_positive_dir: + homing_dir = 1 + else: + homing_dir = -1 + #homing is homing_dir + homing_dist = homing_dist * homing_dir + #retract dir is opposite of homing dir + homing_retract_dist = self.homing_retract_dist * homing_dir * -1 + #set position to 0 + self.do_set_position(0.0) + #primary homing move + self.do_homing_move( + homing_dist, + self.homing_speed, + self.homing_accel, + True, + True, + ) + #retract move + logging.info("homing_retract_dist: %s", homing_retract_dist) + pos = self.get_position() + movepos = pos[0] + homing_retract_dist + self.do_manual_move(movepos, self.homing_retract_speed, self.homing_accel) + self.do_set_position(0.0) + self.do_homing_move( + homing_dist, + self.second_homing_speed, + accel, + True, + True, + ) + + #need to move to actual 0, and we are at position_endstop + self.do_set_position(self.position_endstop) + self.do_manual_move(0, self.homing_retract_speed, self.homing_accel) + + + + def do_homing_move(self, movepos, speed, accel, triggered, check_trigger): + logging.info("do_homing_move") + logging.info("movepos: %s", movepos) + logging.info("speed: %s", speed) + logging.info("accel: %s", accel) + if not self.can_home: + raise self.printer.command_error( + "No endstop for this stepper" + ) + pos = [movepos, 0.0, 0.0, 0.0] + endstops = self.endstops + phoming = self.printer.lookup_object("homing") + phoming.manual_home( + self, endstops, pos, speed, triggered, check_trigger + ) + + def add_endstop(self, config): + stepper = self + # if self.endstops and config.get("endstop_pin", None) is None: + # # No endstop defined - use primary endstop + # self.endstops[0][0].add_stepper(stepper) + # return + endstop_pin = config.get("endstop_pin") + printer = config.get_printer() + ppins = printer.lookup_object("pins") + pin_params = ppins.parse_pin(endstop_pin, True, True) + # Normalize pin name + pin_name = "%s:%s" % (pin_params["chip_name"], pin_params["pin"]) + # Look for already-registered endstop + # New endstop, register it + mcu_endstop = ppins.setup_pin("endstop", endstop_pin) + self.endstop_map[pin_name] = { + "endstop": mcu_endstop, + "invert": pin_params["invert"], + "pullup": pin_params["pullup"], + } + mcu_endstop.add_stepper(stepper) + name = stepper.get_name(short=True) + self.endstops.append((mcu_endstop, name)) + query_endstops = printer.load_object(config, "query_endstops") + query_endstops.register_endstop(mcu_endstop, name) + + self.position_endstop = config.getfloat("position_endstop") + self.homing_speed = config.getfloat("homing_speed", 100.0, above=0.0) + self.second_homing_speed = config.getfloat( + "second_homing_speed", self.homing_speed / 2.0, above=0.0 + ) + self.homing_retract_speed = config.getfloat( + "homing_retract_speed", self.homing_speed, above=0.0 + ) + self.homing_retract_dist = config.getfloat( + "homing_retract_dist", 20, minval=0.0 + ) + self.homing_speed = math.radians(self.homing_speed) + self.second_homing_speed = math.radians(self.second_homing_speed) + self.homing_retract_speed = math.radians(self.homing_retract_speed) + self.homing_retract_dist = math.radians(self.homing_retract_dist) + self.position_endstop = math.radians(self.position_endstop) + + self.homing_positive_dir = config.getboolean( + "homing_positive_dir", False + ) + # self.axis = "X" + + # Toolhead wrappers to support homing + def flush_step_generation(self): + logging.info("flush_step_generation") + self.sync_print_time() + + def get_position(self): + # axis_index = "XYZ".find(self.axis) + # pos = [0.0, 0.0, 0.0, 0.0] + # [pos[axis_index]] = self.get_commanded_position() + # return pos + + + pos = [self.get_commanded_position(), 0.0, 0.0] + logging.info("homeable_stepper pos: %s", pos) + return pos + + # def set_position(self, newpos, homing_axes=()): + # self.do_set_position(newpos[0]) + + def get_last_move_time(self): + logging.info("get_last_move_time") + self.sync_print_time() + return self.next_cmd_time + + def dwell(self, delay): + self.next_cmd_time += max(0.0, delay) + + def drip_move(self, newpos, speed, drip_completion): + try: + self.do_manual_move(newpos[0], speed, self.homing_accel, sync=True, drip_completion=drip_completion) + except DripModeEndSignal as e: + logging.info("drip signal! finalizing moves.") + self.trapq_finalize_moves(self._trapq, self.reactor.NEVER) + def get_kinematics(self): + return self + + def get_steppers(self): + return [self] + + def calc_position(self, stepper_positions): + return [stepper_positions[self.get_name()], 0.0, 0.0] + + +def HomeablePrinterStepper(config): + units_in_radians = True + printer = config.get_printer() + name = config.get_name() + # Stepper definition + ppins = printer.lookup_object("pins") + step_pin = config.get("step_pin") + step_pin_params = ppins.lookup_pin(step_pin, can_invert=True) + dir_pin = config.get("dir_pin") + dir_pin_params = ppins.lookup_pin(dir_pin, can_invert=True) + rotation_dist, steps_per_rotation = parse_step_distance( + config, units_in_radians, True + ) + step_pulse_duration = config.getfloat( + "step_pulse_duration", None, minval=0.0, maxval=0.001 + ) + mcu_stepper = HomeableStepper( + config, + name, + step_pin_params, + dir_pin_params, + rotation_dist, + steps_per_rotation, + step_pulse_duration, + units_in_radians, + ) + # Register with helper modules + for mname in ["stepper_enable", "force_move", "motion_report"]: + m = printer.load_object(config, mname) + m.register_stepper(config, mcu_stepper) + return mcu_stepper diff --git a/klippy/extras/homing.py b/klippy/extras/homing.py index adfd9d062..9ad275d42 100644 --- a/klippy/extras/homing.py +++ b/klippy/extras/homing.py @@ -82,6 +82,13 @@ def calc_toolhead_pos(self, kin_spos, offsets): for stepper in kin.get_steppers(): sname = stepper.get_name() kin_spos[sname] += offsets.get(sname, 0) * stepper.get_step_dist() + logging.info("step dist: %s", stepper.get_step_dist()) + logging.info("kin_spos: %s", kin_spos) + cs = [ + (s.get_name(), s.get_commanded_position()) + for s in kin.get_steppers() + ] + logging.info("commanded_poses: %s", cs) thpos = self.toolhead.get_position() return list(kin.calc_position(kin_spos))[:3] + thpos[3:] @@ -154,8 +161,11 @@ def homing_move( for sp in self.stepper_positions } haltpos = trigpos = self.calc_toolhead_pos(kin_spos, trig_steps) + logging.info("probe trigpos: %s", trigpos) if trig_steps != halt_steps: + logging.info("using halt steps") haltpos = self.calc_toolhead_pos(kin_spos, halt_steps) + logging.info("probe haltpos: %s", haltpos) else: haltpos = trigpos = movepos over_steps = { @@ -173,12 +183,15 @@ def homing_move( } self.distance_elapsed = kin.calc_position(filled_steps_moved) if any(over_steps.values()): + logging.info("movepos: %s", movepos) + self.toolhead.set_position(movepos) halt_kin_spos = { s.get_name(): s.get_commanded_position() for s in kin.get_steppers() } haltpos = self.calc_toolhead_pos(halt_kin_spos, over_steps) + logging.info("haltpos: %s", haltpos) self.toolhead.set_position(haltpos) # Signal homing/probing move complete try: @@ -194,6 +207,10 @@ def check_no_movement(self): if self.printer.get_start_args().get("debuginput") is not None: return None for sp in self.stepper_positions: + logging.info("sp endstop name: %s", sp.endstop_name) + logging.info("sp stepper name: %s", sp.stepper_name) + logging.info("sp start_pos: %s", sp.start_pos) + logging.info("sp halt_pos: %s", sp.halt_pos) if sp.start_pos == sp.trig_pos: return sp.endstop_name return None diff --git a/klippy/extras/manual_probe.py b/klippy/extras/manual_probe.py index 709fd4463..70f44d815 100644 --- a/klippy/extras/manual_probe.py +++ b/klippy/extras/manual_probe.py @@ -211,6 +211,7 @@ def move_z(self, z_pos): def report_z_status(self, warn_no_change=False, prev_pos=None): # Get position kin_pos = self.get_kinematics_pos() + logging.info("kinematics pos: %s", kin_pos) z_pos = kin_pos[2] if warn_no_change and z_pos == prev_pos: self.gcode.respond_info( diff --git a/klippy/extras/polar_alignment.py b/klippy/extras/polar_alignment.py new file mode 100644 index 000000000..577db57dd --- /dev/null +++ b/klippy/extras/polar_alignment.py @@ -0,0 +1,132 @@ +import chelper +import logging +import math + + +def polar_to_cartesian(r, theta): + return (r * math.cos(theta), r * math.sin(theta)) + +def circle_from_3_points(p1, p2, p3): + z1 = complex(p1[0], p1[1]) + z2 = complex(p2[0], p2[1]) + z3 = complex(p3[0], p3[1]) + + if (z1 == z2) or (z2 == z3) or (z3 == z1): + raise ValueError("Duplicate points: %s, %s, %s" % (z1, z2, z3)) + + w = (z3 - z1) / (z2 - z1) + + if abs(w.imag) <= 0.0001: + raise ValueError("Points are collinear: %s, %s, %s" % (z1, z2, z3)) + + c = (z2 - z1) * (w - abs(w) ** 2) / (2j * w.imag) + z1 + # Simplified denominator + r = abs(z1 - c) + r = round(r, 10) + c = (round(c.real, 10), round(c.imag, 10)) + + return c, r + +class PolarAlignment: + def __init__(self, config): + self.printer = config.get_printer() + self.steppers = {} + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command('POLAR_ALIGN', self.cmd_POLAR_ALIGN, + desc=self.cmd_POLAR_ALIGN_help) + self.gcode.register_command('PROBETEST', self.cmd_PROBETEST, + desc=self.cmd_POLAR_ALIGN_help) + self.bearing_diameter = config.getfloat( + "bearing_diameter", 60, above=0.0 + ) + self.threads = { 'CW-M3': 0.5, 'CW-M4': 0.7, 'CW-M5': 0.8 } + self.thread_factor = config.getchoice('screw_thread', self.threads, + default='CW-M5') + self.z_probe_position = config.getfloat( + "z_probe_position", 10, above=0.0 + ) + self.z_clearance = config.getfloat( + "z_clearance", 50, above=0.0 + ) + self.center_offset = config.getfloat( + "center_offset", 25, above=0.0 + ) + self.lower_clearance = config.getfloat( + "lower_clearance", 1, above=0.0 + ) + self.bed_stepper = config.getsection("stepper_x") + self.gcode.respond_info("bed_stepper: %s" % self.bed_stepper) + self.probing_speed = config.getfloat('probing_speed', 10.0, above=0.) + self.mcu_probe = self.printer.lookup_object('probe').mcu_probe + + def register_stepper(self, config, mcu_stepper): + self.steppers[mcu_stepper.get_name()] = mcu_stepper + def lookup_stepper(self, name): + if name not in self.steppers: + raise self.printer.config_error("Unknown stepper %s" % (name,)) + return self.steppers[name] + cmd_POLAR_ALIGN_help = "Tool to help align bed" + def cmd_PROBETEST(self, gcmd): + self.toolhead = self.printer.lookup_object('toolhead') + res = self._bearing_probe() + self.gcode.respond_info("distance is %s" % res) + + def _move(self, pos, speed): + self.toolhead.move((pos[0], pos[1], pos[2], 0), speed) + + def _bearing_probe(self): + phoming = self.printer.lookup_object('homing') + move_speed = self.printer.lookup_object('gcode_move').speed + self._move((self.center_offset,0,self.z_clearance), move_speed) + logging.info("moving to %s" % self.center_offset) + self._move((self.center_offset,0,self.z_probe_position), move_speed) + probe = self.printer.lookup_object('probe') + probe_gcmd = self.gcode.create_gcode_command( + "PROBE", "PROBE", {}) + result = probe.run_probe(probe_gcmd) + z_probe_height = result[2] + x_probe_height = z_probe_height + self.lower_clearance + self._move((self.center_offset, 0, x_probe_height), move_speed) + position = ((self.bearing_diameter / 2) + 10, 0, x_probe_height, 0) + epos = phoming.probing_move(self.mcu_probe, position, self.probing_speed) + dist = epos[0] + self.center_offset + self.gcode.respond_info("probe was %s" % dist) + self._move((self.center_offset,0,self.z_clearance), move_speed) + return dist + + def cmd_POLAR_ALIGN(self, gcmd): + self.toolhead = self.printer.lookup_object('toolhead') + move_speed = self.printer.lookup_object('gcode_move').speed + dist_1 = self._bearing_probe() + self.move_bed(120, 120) + dist_2 = self._bearing_probe() + self.move_bed(120, 120) + dist_3 = self._bearing_probe() + self.move_bed(120, 120) + + + p1 = polar_to_cartesian(dist_1, 0) + p2 = polar_to_cartesian(dist_2, 120) + p3 = polar_to_cartesian(dist_3, 240) + center, _ = circle_from_3_points(p1, p2, p3) + x_offset = center[0] + y_offset = center[1] + offset_offset = 25 + self._move((x_offset + offset_offset,0,self.z_clearance), move_speed) + self.toolhead.set_position((offset_offset,0,self.z_clearance, 0)) + self.gcode.respond_info("x was off by %s" % x_offset) + self.gcode.respond_info("y is off by %s" % y_offset) + self.gcode.respond_info("01:20 means 1 full turn and 20 minutes") + adjust = y_offset / self.thread_factor + adjust = abs(adjust) + full_turns = math.trunc(adjust) + decimal_part = adjust - full_turns + minutes = round(decimal_part * 60, 0) + self.gcode.respond_info("Adjust by %s:%s" % (full_turns, minutes)) + + def move_bed(self, angle, speed, accel=1000): + self.printer.lookup_object('force_move').manual_move( + self.lookup_stepper('stepper_bed'), angle, speed, accel) + +def load_config(config): + return PolarAlignment(config) diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py index 1e581b291..ceb4b3c2d 100644 --- a/klippy/extras/probe.py +++ b/klippy/extras/probe.py @@ -217,6 +217,7 @@ def run_probe(self, gcmd): first_probe = True while len(positions) < sample_count: # Probe position + logging.info("probing!") pos = self._probe(speed) if self.drop_first_result and first_probe: first_probe = False @@ -234,6 +235,7 @@ def run_probe(self, gcmd): positions = [] # Retract if len(positions) < sample_count: + logging.info("retracting!") self._move(probexy + [pos[2] + sample_retract_dist], lift_speed) if must_notify_multi_probe: self.multi_probe_end() diff --git a/klippy/extras/screws_tilt_adjust.py b/klippy/extras/screws_tilt_adjust.py index 1afd2bb0d..3210a0bfd 100644 --- a/klippy/extras/screws_tilt_adjust.py +++ b/klippy/extras/screws_tilt_adjust.py @@ -17,14 +17,26 @@ def __init__(self, config): self.max_diff = None self.max_diff_error = False # Read config - for i in range(99): - prefix = "screw%d" % (i + 1,) - if config.get(prefix, None) is None: - break - screw_coord = config.getfloatlist(prefix, count=2) - screw_name = "screw at %.3f,%.3f" % screw_coord - screw_name = config.get(prefix + "_name", screw_name) - self.screws.append((screw_coord, screw_name)) + generate_screw_positions = config.getboolean( + 'generate_polar_positions', False) + if generate_screw_positions: + screw_radius = config.getfloat('screw_radius', above=0.) + num_screws = config.getint('screw_count', minval=3) + initial_angle = 0 + for i in range(num_screws): + angle = initial_angle + (i * 2 * math.pi / num_screws) + x = screw_radius * math.cos(angle) + y = screw_radius * math.sin(angle) + self.screws.append(([x, y], "screw%d" % (i + 1,))) + else: + for i in range(99): + prefix = "screw%d" % (i + 1,) + if config.get(prefix, None) is None: + break + screw_coord = config.getfloatlist(prefix, count=2) + screw_name = "screw at %.3f,%.3f" % screw_coord + screw_name = config.get(prefix + "_name", screw_name) + self.screws.append((screw_coord, screw_name)) if len(self.screws) < 3: raise config.error( "screws_tilt_adjust: Must have " "at least three screws" @@ -43,7 +55,7 @@ def __init__(self, config): "screw_thread", self.threads, default="CW-M3" ) # Initialize ProbePointsHelper - points = [coord for coord, name in self.screws] + points = self._generate_points() self.probe_helper = probe.ProbePointsHelper( self.config, self.probe_finalize, default_points=points ) @@ -62,6 +74,17 @@ def __init__(self, config): "of turns to level it." ) + def _generate_points(self): + # kin = self.printer.lookup_object('toolhead').get_kinematics() + # if hasattr(kin, 'stepper_bed'): + # # polar + # radius = kin.radius + # points = + # else: + + points = [coord for coord, name in self.screws] + return points + def cmd_SCREWS_TILT_CALCULATE(self, gcmd): self.max_diff = gcmd.get_float("MAX_DEVIATION", None) # Option to force all turns to be in the given direction (CW or CCW) diff --git a/klippy/kinematics/polarxz.py b/klippy/kinematics/polarxz.py new file mode 100644 index 000000000..d77dda89b --- /dev/null +++ b/klippy/kinematics/polarxz.py @@ -0,0 +1,1280 @@ +# Code for handling the kinematics of polar robots +# +# Copyright (C) 2018-2021 Kevin O'Connor +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import stepper +import sys +import logging, math +from collections import OrderedDict +import chelper +from extras.homeable_stepper import HomeablePrinterStepper +from shapely.geometry import LineString +from shapely.geometry import Point +from shapely.geometry import MultiPoint + +EPSILON = 0.000001 +ROUND_CONSTANT = 8 +CALC_POS_ROUND_CONSTANT = 4 +HALF_PI = math.pi * 0.5 +BED_CENTER = (0, 0) + + +def get_region_indices(start, mid, end, regions): + # regions is expected to be sorted in descending order + # loop through the points and find the smallest region that contains each point + regions_list = [] + sqred_regions = [round(r**2, ROUND_CONSTANT) for r in regions] + for point in [start, mid, end]: + smallest_r_idx = None + point_dist_sq = round(point[0] ** 2 + point[1] ** 2, ROUND_CONSTANT) + for i, r_sq in enumerate(sqred_regions): + if point_dist_sq <= r_sq: + smallest_r_idx = i + regions_list.append(smallest_r_idx) + + return regions_list + + +def distance(p1, p2): + return math.sqrt(((p2[0] - p1[0]) ** 2) + ((p2[1] - p1[1]) ** 2)) + + +def sqrdistance(p1, p2): + return ((p2[0] - p1[0]) ** 2) + ((p2[1] - p1[1]) ** 2) + + +def cartesian_to_polar(x, y): + return (math.sqrt(x**2 + y**2), math.atan2(y, x)) + + +def polar_to_cartesian(r, theta): + return (r * math.cos(theta), r * math.sin(theta)) + + +def get_quadrant_crosses(p1, p2): + return int(abs(p2[1] - p1[1]) // HALF_PI) + + +# def alignment_wip(): +# # GCODE: set current xy position as 0,0 +# angle_1 = 0 +# angle_2 = 120 +# angle_3 = 240 + +# dist1 = probe_bearing(angle_1) +# dist2 = probe_bearing(angle_2) +# dist3 = probe_bearing(angle_3) + +# p1 = (dist1, angle_1) +# p2 = (dist2, angle_2) +# p3 = (dist3, angle_3) +# # p1, p2 and p3 are polar coordinates +# cartesian_p1 = polar_to_cartesian(p1[0], p1[1]) +# cartesian_p2 = polar_to_cartesian(p2[0], p2[1]) +# cartesian_p3 = polar_to_cartesian(p3[0], p3[1]) +# circle = circle_from_3_points(cartesian_p1, cartesian_p2, cartesian_p3) +# circle_center = circle[0] +# # it's assumed that we started at 0,0. +# # so the circle center is 0 - the offset in x and y. +# x_offset = -circle_center[0] +# y_offset = -circle_center[1] +# # GCODE: move radius arm to (x_offset) +# # GCODE: spit back Y offset to calibration command + +def get_circle_line_intersections_newnew(p1, p2, distance): + p = Point(0,0) + c = p.buffer(distance, resolution=25).boundary + l = LineString([(p1[0], p1[1]), (p2[0], p2[1])]) + i = c.intersection(l) + + if isinstance(i, Point): + intersection = i.coords[0] + return [(intersection[0], intersection[1]),] + elif isinstance(i, MultiPoint): + intersection1 = i.geoms[0].coords[0] + intersection2 = i.geoms[1].coords[0] + return [(intersection1[0], intersection1[1]), (intersection2[0], intersection2[1])] + else: + return [] + +def get_circle_line_intersections_new(p1, p2, dist): + # find points on a line that are a given distance from origin + intersections = [] + x1 = p1[0] + y1 = p1[1] + x2 = p2[0] + y2 = p2[1] + + m = (y2 - y1) / (x2 - x1) + # calculate y intercept + b = y1 - m * x1 + new_x1 = (-(m * x1 + b) + math.sqrt((m * x1 + b) ** 2 - dist**2)) / 2 + new_x2 = (-(m * x1 + b) - math.sqrt((m * x1 + b) ** 2 - dist**2)) / 2 + new_y1 = m * new_x1 + b + new_y2 = m * new_x2 + b + + intersection1 = (new_x1, new_y1) + intersection2 = (new_x2, new_y2) + + if ( + p1[0] <= intersection1[0] <= p2[0] or p2[0] <= intersection1[0] <= p1[0] + ) and ( + p1[1] <= intersection1[1] <= p2[1] or p2[1] <= intersection1[1] <= p1[1] + ): + intersections.append(intersection1) + if ( + p1[0] <= intersection2[0] <= p2[0] or p2[0] <= intersection2[0] <= p1[0] + ) and ( + p1[1] <= intersection2[1] <= p2[1] or p2[1] <= intersection2[1] <= p1[1] + ): + intersections.append(intersection2) + + return intersections + + +def get_circle_line_intersections(p1, p2, radius): + # calculate the intersection points of a line and circle + # https://stackoverflow.com/questions/1073336/circle-line-collision-detection + # p1, p2 are the endpoints of the line + # radius is the radius of the circle + p1 = (round(p1[0], ROUND_CONSTANT), round(p1[1], ROUND_CONSTANT)) + p2 = (round(p2[0], ROUND_CONSTANT), round(p2[1], ROUND_CONSTANT)) + intersections = [] + dx = p2[0] - p1[0] + dy = p2[1] - p1[1] + dr = math.sqrt(dx**2 + dy**2) + D = (p1[0] * p2[1]) - (p2[0] * p1[1]) + disc = (radius**2) * (dr**2) - (D**2) + if disc < 0: + return intersections + elif disc == 0: + x = (D * dy) / (dr**2) + y = (-D * dx) / (dr**2) + intersections.append((x, y)) + else: + x1 = ((D * dy) + (math.copysign(1, dy) * dx * math.sqrt(disc))) / ( + dr**2 + ) + x2 = ((D * dy) - (math.copysign(1, dy) * dx * math.sqrt(disc))) / ( + dr**2 + ) + y1 = ((-D * dx) + (abs(dy) * math.sqrt(disc))) / (dr**2) + y2 = ((-D * dx) - (abs(dy) * math.sqrt(disc))) / (dr**2) + x1 = round(x1, ROUND_CONSTANT) + x2 = round(x2, ROUND_CONSTANT) + y1 = round(y1, ROUND_CONSTANT) + y2 = round(y2, ROUND_CONSTANT) + intersection1 = (x1, y1) + intersection2 = (x2, y2) + if ( + p1[0] <= intersection1[0] <= p2[0] + or p2[0] <= intersection1[0] <= p1[0] + ) and ( + p1[1] <= intersection1[1] <= p2[1] + or p2[1] <= intersection1[1] <= p1[1] + ): + intersections.append(intersection1) + if ( + p1[0] <= intersection2[0] <= p2[0] + or p2[0] <= intersection2[0] <= p1[0] + ) and ( + p1[1] <= intersection2[1] <= p2[1] + or p2[1] <= intersection2[1] <= p1[1] + ): + intersections.append(intersection2) + + return intersections + + +def get_quadrant_info(p1, p2): + angles_crossed = [] + start_quadrant = int(p1[1] // HALF_PI) + end_quadrant = int(p2[1] // HALF_PI) + num_quadrants_crossed = abs(start_quadrant - end_quadrant) + if num_quadrants_crossed == 0: + return angles_crossed + else: + angles_crossed = list( + range( + (start_quadrant + 1) * HALF_PI, + 1 + (end_quadrant * HALF_PI), + HALF_PI, + ) + ) + return num_quadrants_crossed, angles_crossed + + +# def generate_segmentation_radii(offset_dist, num_thresholds): + +# radius = 100 +# threshold_size = radius / num_thresholds +# nums = [] +# for i in range(num_thresholds - 1): +# radius -= threshold_size +# nums.append(radius) +# nums.append(offset_dist) +# return nums +def generate_segmentation_radii(offset_dist, rate, max_pos=100): + dist = max_pos + nums = [] + while dist > offset_dist: + newdist = dist * rate + nums.append(dist) + dist = newdist + if nums[-1] != offset_dist: + nums.append(offset_dist) + return nums + + +def distance_point_to_line(p0, p1, p2): + return abs( + ((p2[0] - p1[0]) * (p1[1] - p0[1])) + - ((p1[0] - p0[0]) * (p2[1] - p1[1])) + ) / distance(p1, p2) + + +class PolarXZKinematics: + def __init__(self, toolhead, config): + # Setup axis steppers + # self.stepper_bed = stepper.PrinterStepper( + # config.getsection("stepper_bed"), units_in_radians=True + # ) + self.debug = config.getboolean("debug", False) + self.toolhead = toolhead + self.stepper_bed = HomeablePrinterStepper( + config.getsection("stepper_bed") + ) + self.setup_bed_itersolve(for_homing=False) + # ffi_main, ffi_lib = chelper.get_ffi() + # self.stepper_bed = None + # self.rail_bed = stepper.PrinterRail(config.getsection("stepper_bed"), units_in_radians=True) + rail_x = stepper.PrinterRail(config.getsection("stepper_x")) + rail_z = stepper.PrinterRail(config.getsection("stepper_z")) + self.debug_log("rail x endstops: %s", rail_x.get_endstops()) + self.debug_log("rail z endstops: %s", rail_z.get_endstops()) + + rail_x.get_endstops()[0][0].add_stepper(rail_z.get_steppers()[0]) + rail_z.get_endstops()[0][0].add_stepper(rail_x.get_steppers()[0]) + # self.manual_stepper_kinematics = ffi_main.gc( + # ffi_lib.cartesian_stepper_alloc(b"x"), ffi_lib.free + # ) + + rail_x.setup_itersolve("polarxz_stepper_alloc", b"+") + rail_z.setup_itersolve("polarxz_stepper_alloc", b"-") + self.rails = [ + rail_x, + rail_z, + ] + self.rail_lookup = { + "x": rail_x, + "z": rail_z, + } + self.steppers = [self.stepper_bed] + [ + s for r in self.rails for s in r.get_steppers() + ] + + for s in self.get_steppers(): + s.set_trapq(toolhead.get_trapq()) + toolhead.register_step_generator(s.generate_steps) + config.get_printer().register_event_handler( + "stepper_enable:motor_off", self._motor_off + ) + # Setup boundary checks + self.max_velocity, self.max_accel = toolhead.get_max_velocity() + # in deg/s + self.max_rotational_velocity = config.getfloat( + "max_rotational_velocity", 360, above=0.0 + ) + + self.max_rotational_accel = config.getfloat( + "max_rotational_accel", + 720, + above=0.0, + maxval=self.max_accel, + ) + + # convert bed velocity/accel limits to radians + self.max_rotational_velocity = math.radians( + self.max_rotational_velocity + ) + self.max_rotational_accel = math.radians(self.max_rotational_accel) + + self.max_z_velocity = config.getfloat( + "max_z_velocity", + self.max_velocity, + above=0.0, + maxval=self.max_velocity, + ) + self.max_z_accel = config.getfloat( + "max_z_accel", self.max_accel, above=0.0, maxval=self.max_accel + ) + self.limit_z = (1.0, -1.0) + self.limit_xy2 = -1.0 + self.max_xy = self.rails[0].get_range()[1] + min_z, max_z = self.rails[1].get_range() + self.axes_min = toolhead.Coord(-self.max_xy, -self.max_xy, min_z, 0.0) + self.axes_max = toolhead.Coord(self.max_xy, self.max_xy, max_z, 0.0) + self.zero_crossing_radius = config.getfloat( + "zero_crossing_radius", 0.1, minval=0.0001, above=0.0 + ) + self.min_segment_length = config.getfloat( + "min_segment_length", 0.2, minval=0.0001, above=0.0 + ) + self.segments_per_second = config.getfloat( + "segments_per_second", 100, minval=0.0001, above=0.0 + ) + self.two_move_crossing = config.getboolean("two_move_crossing", False) + self.time_segmentation = config.getboolean("time_segmentation", False) + self.bed_radius = config.getfloat("bed_radius", 100, above=1.0) + self.homing = False + + self.segmentation_radii = generate_segmentation_radii( + self.zero_crossing_radius, 0.5, round(self.max_xy, 0) + 5 + ) + + + def debug_log(self, *args, **kwargs): + if self.debug: + logging.info(*args, **kwargs) + + def setup_bed_itersolve(self, for_homing=False): + if for_homing: + self.stepper_bed.setup_itersolve_for_homing() + else: + self.stepper_bed.setup_itersolve("polarxz_stepper_alloc", b"a") + self.stepper_bed.set_trapq(self.toolhead.get_trapq()) + def get_steppers(self): + return list(self.steppers) + + def calc_position(self, stepper_positions): + bed_angle = stepper_positions[self.steppers[0].get_name()] + x_pos = stepper_positions[self.rails[0].get_name()] + z_pos = stepper_positions[self.rails[1].get_name()] + radius = 0.5 * (x_pos + z_pos) + z_pos = 0.5 * (x_pos - z_pos) + x_pos = radius * math.cos(bed_angle) + y_pos = radius * math.sin(bed_angle) + return [ + round(x_pos, ROUND_CONSTANT), + round(y_pos, ROUND_CONSTANT), + round(z_pos, ROUND_CONSTANT), + ] + + def set_position(self, newpos, homing_axes): + self.debug_log("polarxz setpos: %s", newpos) + for s in self.steppers: + new_newpos = tuple([round(val, ROUND_CONSTANT) for val in newpos]) + # x = new_newpos[0] + # y = new_newpos[1] + # dist = distance((x, y), (0, 0)) + # if dist != 0 and dist < self.zero_crossing_radius: + # # get normalized vector + # x = x / dist + # y = y / dist + # # scale to zero crossing radius + # x = x * self.zero_crossing_radius + # y = y * self.zero_crossing_radius + # new_newpos = (x, y) + tuple(new_newpos[2:]) + # self.debug_log( + # "polarxz setpos less than radius, but now: %s", new_newpos + # ) + s.set_position(new_newpos) + if 2 in homing_axes: + self.limit_z = self.rails[1].get_range() + if 0 in homing_axes and 1 in homing_axes: + self.limit_xy2 = self.rails[0].get_range()[1] ** 2 + + def note_z_not_homed(self): + # Helper for Safe Z Home + self.limit_z = (1.0, -1.0) + + # def _home_axis(self, homing_state, axis, rail): + # # Determine movement + # position_min, position_max = rail.get_range() + # hi = rail.get_homing_info() + # homepos = [None, None, None, None] + # homepos[axis] = hi.position_endstop + # if axis == 0: + # homepos[1] = 0.0 + # forcepos = list(homepos) + # if hi.positive_dir: + # forcepos[axis] -= hi.position_endstop - position_min + # else: + # forcepos[axis] += position_max - hi.position_endstop + # # Perform homing + # homing_state.home_rails([rail], forcepos, homepos) + + # def home(self, homing_state): + # # Always home XY together + # homing_axes = homing_state.get_axes() + # home_xy = 0 in homing_axes or 1 in homing_axes + # home_z = 2 in homing_axes + # updated_axes = [] + # if home_xy: + # updated_axes = [0, 1] + # if home_z: + # updated_axes.append(2) + # homing_state.set_axes(updated_axes) + # # Do actual homing + # if home_xy: + # self._home_axis(homing_state, 0, self.rails[0]) + # # self._home_axis(homing_state, 1, self.rails[0]) + # if home_z: + # self._home_axis(homing_state, 2, self.rails[1]) + def _home_bed(self, homing_state): + self.setup_bed_itersolve(for_homing=True) + self.stepper_bed.home(self.max_rotational_accel) + self.setup_bed_itersolve(for_homing=False) + self.debug_log("bed homed! should set itersolve back.") + def home(self, homing_state): + self.homing = True + # Each axis is homed independently and in order + homing_axes = homing_state.get_axes() + # home_xy = 0 in homing_axes or 1 in homing_axes + home_x = 0 in homing_axes + home_y = 1 in homing_axes + home_z = 2 in homing_axes + updated_axes = [] + if home_z: + updated_axes.append(2) + if home_x: + updated_axes.append(0) + if home_y: + updated_axes.append(1) + homing_state.set_axes(updated_axes) + for axis in updated_axes: + if axis == 2: # if we're homing z, get the z rail + self.debug_log("homing z!") + rail = self.rails[1] + elif axis == 1: # y doesn't do shit + # home bed?! + self.debug_log("we should be homing Y") + self._home_bed(homing_state) + continue + elif axis == 0: + self.debug_log("homing x!") + rail = self.rails[0] + # Determine movement + position_min, position_max = rail.get_range() + hi = rail.get_homing_info() + homepos = [None, None, None, None] + homepos[axis] = hi.position_endstop + if axis == 0: + homepos[1] = 0.0 + self.debug_log("homepos: %s", homepos) + forcepos = list(homepos) + self.debug_log("forcepos: %s", forcepos) + self.debug_log("hi.positive_dir: %s", hi.positive_dir) + self.debug_log("hi.position_endstop: %s", hi.position_endstop) + self.debug_log("position_min: %s", position_min) + self.debug_log("position_max: %s", position_max) + if axis == 0: + if hi.positive_dir: + # klipper dies if we do a move at 0,0, so offset position by microstep distance + # TODO - maybe only offset if it's an x move + forcepos[axis] -= ( + hi.position_endstop + - position_min + - self.zero_crossing_radius + ) + if forcepos[axis] < position_min: + forcepos[axis] = position_min + self.zero_crossing_radius + else: + forcepos[axis] += ( + position_max + - hi.position_endstop + + self.zero_crossing_radius + ) + if forcepos[axis] > position_max: + forcepos[axis] = position_max - self.zero_crossing_radius + else: + if hi.positive_dir: + forcepos[axis] = position_min + else: + forcepos[axis] = position_max + + # Perform homing + self.debug_log("homing axis: %s, forcepos: %s, homepos: %s", axis, forcepos, homepos) + homing_state.home_rails([rail], forcepos, homepos) + self.homing = False + def _motor_off(self, print_time): + self.limit_z = (1.0, -1.0) + self.limit_xy2 = -1.0 + + def check_move(self, move): + if self.homing: + return move + end_pos = move.end_pos + xy2 = end_pos[0] ** 2 + end_pos[1] ** 2 + if xy2 > self.limit_xy2: + if self.limit_xy2 < 0.0: + raise move.move_error("Must home axis first") + raise move.move_error() + # Limit the maximum acceleration against the rotational distance theta + # TODO: Optimize with code from the chelper? + if move.axes_d[0] or move.axes_d[1]: + start_xy = move.start_pos + end_xy = move.end_pos + # accel = move.accel + # delta_x = end_xy[0] - start_xy[0] + # delta_y = end_xy[1] - start_xy[1] + # calculate accel components of x and y from deltas + # move_hypot = math.sqrt(delta_x**2 + delta_y**2) + # accel_x = accel * delta_x / move_hypot + # accel_y = accel * delta_y / move_hypot + # convert accel x and accel y to polar components + # accel_theta = math.atan2(accel_y, accel_x) + # accel_r = math.sqrt(accel_x**2 + accel_y**2) + polar_start = cartesian_to_polar(start_xy[0], start_xy[1]) + polar_end = cartesian_to_polar(end_xy[0], end_xy[1]) + # dr = polar_end[0] - polar_start[0] + + # dt = move.min_move_t + + dtheta = polar_end[1] - polar_start[1] + + delta_degrees = math.degrees(dtheta) + if abs(delta_degrees) > 180: + if delta_degrees > 0: + delta_degrees = delta_degrees - 360 + else: + delta_degrees = delta_degrees + 360 + if delta_degrees == 0: + step_ratio = self.max_accel / self.max_rotational_accel + else: + steps_per_degree = 360 / 1.8 * (16 / 120.0) + delta_distance = distance(move.start_pos, move.end_pos) + step_ratio = ( + delta_distance * steps_per_degree / abs(delta_degrees) + ) + self.debug_log("delta_degrees: %s" % delta_degrees) + self.debug_log("delta_distance: %s" % delta_distance) + self.debug_log("steps_per_degree: %s" % steps_per_degree) + + # rotational_velocity = dtheta / dt + # radial_velocity = dr / dt + # if radial_velocity == 0: + # r = polar_start[0] + # #calculate sagitta + # sagitta = r - math.sqrt((r**2) - ((distance(move.start_pos, move.end_pos)/2)**2)) + # radial_velocity = sagitta / dt + + # radius_scale = min(polar_start[0], polar_end[0]) / self.bed_radius + + # rotational_velocity = rotational_velocity * radius_scale + # if rotational_velocity > self.max_rotational_velocity: + # rotational_velocity = self.max_rotational_velocity + + # vx = (radial_velocity * math.cos(polar_start[1])) - (polar_start[0] * rotational_velocity * math.sin(polar_start[1])) + # vy = (radial_velocity * math.sin(polar_start[1])) + (polar_start[0] * rotational_velocity * math.cos(polar_start[1])) + + # adjusted_velocity = math.sqrt(vx**2 + vy**2) + # self.debug_log("adjusted velocity: %s", adjusted_velocity) + self.debug_log("step_ratio: %s" % step_ratio) + + # move.limit_speed(adjusted_velocity, self.max_rotational_accel) + self.debug_log("step_ratio: %s" % step_ratio) + self.debug_log("step ratio * max rotational velocity: %s" % abs(step_ratio * self.max_rotational_velocity)) + self.debug_log("step ratio * max rotational accel: %s" % abs(step_ratio * self.max_rotational_accel)) + move.limit_speed( + abs(step_ratio * self.max_rotational_velocity), + abs(step_ratio * self.max_rotational_accel), + ) + + if move.axes_d[2]: + if end_pos[2] < self.limit_z[0] or end_pos[2] > self.limit_z[1]: + if self.limit_z[0] > self.limit_z[1]: + raise move.move_error("Must home axis first") + raise move.move_error() + # Move with Z - update velocity and accel for slower Z axis + z_ratio = move.move_d / abs(move.axes_d[2]) + move.limit_speed( + self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio + ) + + def zero_cross(self, move, total_move_dist): + cart_start_x = move.start_pos[0] + cart_start_y = move.start_pos[1] + cart_end_x = move.end_pos[0] + cart_end_y = move.end_pos[1] + + zero_cross_radius = self.zero_crossing_radius + # if (abs(cart_start_x) < zero_cross_radius and abs(cart_start_y) < zero_cross_radius): + # zero_cross_radius = distance((cart_start_x, cart_start_y), (0, 0)) + # self.debug_log("zero_cross_radius: %s" % zero_cross_radius) + zero_radius_intersections = get_circle_line_intersections( + (cart_start_x, cart_start_y), + (cart_end_x, cart_end_y), + zero_cross_radius, + ) + self.debug_log( + "zero_radius_intersections: %s" % zero_radius_intersections + ) + + if len(zero_radius_intersections) == 0: + # not crossing zero + return [((move.start_pos, move.end_pos), 1)] + elif len(zero_radius_intersections) == 1: + intersection = zero_radius_intersections[0] + if ( + abs(intersection[0] - cart_start_x) < EPSILON + and abs(intersection[1] - cart_start_y) < EPSILON + ): + return [((move.start_pos, move.end_pos), 1)] + return [ + ( + ( + move.start_pos, + ( + intersection[0], + intersection[1], + move.end_pos[2], + move.end_pos[3], + ), + ), + 1, + ) + ] + # 2 intersections means moving through 0 + # 2 MOVE ZERO CROSSING + elif self.two_move_crossing: + + # moving through our zero radius, move 90 deg to incoming line + # if we know we're zero crossing, the angle to start and to end will always be pi (180deg) apart + # we want to find a point perpendicular to the line between start and end, at a distance of self.zero_crossing_radius + incoming_angle = math.atan2( + zero_radius_intersections[0][1], zero_radius_intersections[0][0] + ) + incoming_point = polar_to_cartesian( + self.zero_crossing_radius, incoming_angle + ) + outgoing_angle = incoming_angle + math.pi + outgoing_point = polar_to_cartesian( + self.zero_crossing_radius, outgoing_angle + ) + side_angle = (outgoing_angle - incoming_angle) / 2 + side_point = polar_to_cartesian( + self.zero_crossing_radius, side_angle + ) + + half_z = move.axes_d[2] / 2 + + half_e = move.axes_d[3] / 2 + side_pos = side_point + ( + move.start_pos[2] + half_z, + move.start_pos[3] + half_e, + ) + return [ + ((move.start_pos, side_pos), 0.5), + ((side_pos, move.end_pos), 0.5), + ] + + # 4 MOVE ZERO CROSSING + else: + # sort zero_radius_intersections by distance from start_pos + zero_radius_intersections.sort( + key=lambda x: sqrdistance((x), move.start_pos) + ) + # moving through our zero radius, move 90 deg to incoming line + # if we know we're zero crossing, the angle to start and to end will always be pi (180deg) apart + # we want to find a point perpendicular to the line between start and end, at a distance of self.zero_crossing_radius + incoming_angle = math.atan2( + zero_radius_intersections[0][1], zero_radius_intersections[0][0] + ) + incoming_point = polar_to_cartesian( + self.zero_crossing_radius, incoming_angle + ) + incoming_point = ( + round(incoming_point[0], ROUND_CONSTANT), + round(incoming_point[1], ROUND_CONSTANT), + ) + outgoing_angle = incoming_angle + math.pi + outgoing_point = polar_to_cartesian( + self.zero_crossing_radius, outgoing_angle + ) + outgoing_point = ( + round(outgoing_point[0], ROUND_CONSTANT), + round(outgoing_point[1], ROUND_CONSTANT), + ) + side_angle = (outgoing_angle - incoming_angle) / 2.0 + side_point = polar_to_cartesian( + self.zero_crossing_radius, side_angle + ) + side_point = ( + round(side_point[0], ROUND_CONSTANT), + round(side_point[1], ROUND_CONSTANT), + ) + crossmove_1 = distance( + (incoming_point[0], incoming_point[1]), + (side_point[0], side_point[1]), + ) + crossmove_2 = distance( + (side_point[0], side_point[1]), + (outgoing_point[0], outgoing_point[1]), + ) + total_move_dist = total_move_dist + total_outermove_dist = total_move_dist - crossmove_1 - crossmove_2 + start_move_dist = end_move_dist = total_outermove_dist / 2.0 + + total_z = move.axes_d[2] + total_e = move.axes_d[3] + start_move_ratio = start_move_dist / float(total_move_dist) + move_1_z_dist = total_z * (start_move_ratio) + move_1_e_dist = total_e * (start_move_ratio) + + crossmove_1_ratio = crossmove_1 / float(total_move_dist) + move_2_z_dist = total_z * (crossmove_1_ratio) + move_2_e_dist = total_e * (crossmove_1_ratio) + + crossmove_2_ratio = crossmove_2 / float(total_move_dist) + move_3_z_dist = total_z * (crossmove_2_ratio) + move_3_e_dist = total_e * (crossmove_2_ratio) + + end_move_ratio = end_move_dist / float(total_move_dist) + move_4_z_dist = total_z * (end_move_ratio) + move_4_e_dist = total_e * (end_move_ratio) + + start_z = move.start_pos[2] + start_e = move.start_pos[3] + + start_pos = move.start_pos + new_z = start_z + move_1_z_dist + new_e = start_e + move_1_e_dist + incoming_pos = (incoming_point[0], incoming_point[1], new_z, new_e) + new_z = new_z + move_2_z_dist + new_e = new_e + move_2_e_dist + side_pos = (side_point[0], side_point[1], new_z, new_e) + new_z = new_z + move_3_z_dist + new_e = new_e + move_3_e_dist + outgoing_pos = (outgoing_point[0], outgoing_point[1], new_z, new_e) + new_z = new_z + move_4_z_dist + new_e = new_e + move_4_e_dist + end_pos = move.end_pos + out_moves = [ + ((start_pos, incoming_pos), start_move_ratio), + ((incoming_pos, side_pos), crossmove_1_ratio), + ((side_pos, outgoing_pos), crossmove_2_ratio), + ((outgoing_pos, end_pos), end_move_ratio), + ] + if ( + abs(out_moves[0][0][0][0] - start_pos[0]) < EPSILON + and abs(out_moves[0][0][0][1] - start_pos[1]) < EPSILON + ): + out_moves.pop(0) + out_moves[0] = ( + (start_pos, out_moves[0][0][1]), + out_moves[0][1], + ) + if ( + abs(out_moves[-1][0][0][0] - end_pos[0]) < EPSILON + and abs(out_moves[-1][0][0][1] - end_pos[1]) < EPSILON + ): + out_moves.pop(-1) + out_moves[-1] = ( + (out_moves[-1][0][0], end_pos), + out_moves[-1][1], + ) + return out_moves + + def segment_move(self, move): + if self.time_segmentation: + return self.segment_move_by_time(move) + else: + return self.segment_move_by_circles(move) + + def segment_move_by_time(self, move): + self.debug_log( + "special_queuing_state: %s", self.toolhead.special_queuing_state + ) + if self.toolhead.special_queuing_state == "Drip": + return [] + if move.axes_d[0] or move.axes_d[1]: + self.debug_log("parent move: %s", (move.start_pos, move.end_pos)) + move_time = move.min_move_t + total_segments = self.segments_per_second * move_time + total_move_dist = distance(move.start_pos, move.end_pos) + moves_and_ratios = self.zero_cross(move, total_move_dist) + out_moves = [] + self.debug_log("moves_and_ratios: %s", moves_and_ratios) + for tup in moves_and_ratios: + + move, ratio = tup + segment_count = int(total_segments * ratio) + move_dist = total_move_dist * ratio + out_moves += self._segment_move(move, segment_count, move_dist) + return out_moves + else: + return [] + + def _segment_move(self, move, num_segments, move_dist): + move_start_pos = move[0] + move_end_pos = move[1] + self.debug_log("segmenting move!") + self.debug_log("move: %s, ", (move_start_pos, move_end_pos)) + dx = move_end_pos[0] - move_start_pos[0] + dy = move_end_pos[1] - move_start_pos[1] + + if num_segments <= 1: + return [move] + segment_dist = move_dist / num_segments + if segment_dist < self.min_segment_length: + num_segments = move_dist / self.min_segment_length + + points = [] + stepx = dx / num_segments + stepy = dy / num_segments + px = move_start_pos[0] + stepx + py = move_start_pos[1] + stepy + self.debug_log("num_segments: %s" % num_segments) + for i in range(int(num_segments)): + + points.append( + (round(px, ROUND_CONSTANT), round(py, ROUND_CONSTANT)) + ) + px += stepx + py += stepy + + points = [(move_start_pos[0], move_start_pos[1])] + points + if len(points) == 1: + points += [(move_end_pos[0], move_end_pos[1])] + self.debug_log("points: %s" % points) + xy_moves = [] + while len(points) != 1: + start = points.pop(0) + end = points[0] + xy_moves.append((start, end)) + self.debug_log("xy_moves: %s" % xy_moves) + total_z_dist = move_end_pos[2] - move_start_pos[2] + self.debug_log("total z dist: %s" % total_z_dist) + total_e_dist = move_end_pos[3] - move_start_pos[3] + actual_moves = [] + current_z_pos = move_start_pos[2] + current_e_pos = move_start_pos[3] + seg_ratio = segment_dist / move_dist + for xy_move in xy_moves: + z_dist = seg_ratio * total_z_dist + e_dist = seg_ratio * total_e_dist + new_z_pos = current_z_pos + z_dist + new_e_pos = current_e_pos + e_dist + actual_moves.append( + ( + ( + round(xy_move[0][0], ROUND_CONSTANT), + round(xy_move[0][1], ROUND_CONSTANT), + round(current_z_pos, ROUND_CONSTANT), + round(current_e_pos, ROUND_CONSTANT), + ), + ( + round(xy_move[1][0], ROUND_CONSTANT), + round(xy_move[1][1], ROUND_CONSTANT), + round(new_z_pos, ROUND_CONSTANT), + round(new_e_pos, ROUND_CONSTANT), + ), + ) + ) + current_e_pos = new_e_pos + current_z_pos = new_z_pos + leftover = round(num_segments % 1, ROUND_CONSTANT) + if leftover > 0: + actual_moves.append((actual_moves[-1][-1], move_end_pos)) + return actual_moves + + def segment_move_by_circles(self, move): + # self.debug_log( + # "special_queuing_state: %s", self.toolhead.special_queuing_state + # ) + # if self.toolhead.special_queuing_state == "Drip": + # return [] + if move.axes_d[0] or move.axes_d[1]: + self.debug_log("move axes_d: %s", move.axes_d) + # def testit(move): + self.debug_log("segmenting move!") + cart_start_x = move.start_pos[0] + cart_start_y = move.start_pos[1] + cart_end_x = move.end_pos[0] + cart_end_y = move.end_pos[1] + delta_x = cart_end_x - cart_start_x + delta_y = cart_end_y - cart_start_y + self.debug_log("start_pos: %s", move.start_pos) + self.debug_log("end_pos: %s", move.end_pos) + + if delta_x == 0: + riserun = 0 + else: + riserun = delta_y / delta_x + + # calculate y intercept + if riserun == 0: + riserun2 = 90 + else: + riserun2 = -1 / riserun + + y_intercept = cart_start_y - (riserun * cart_start_x) + # calculate x intercept + # line1 = y = riserun * x + y_intercept + # line2 = y = riserun2 * x + 0 + # calcualate intersection of two lines + x_intersect = (y_intercept) / (riserun2 - riserun) + y_intersect = riserun * x_intersect + y_intercept + closest_to_origin = (x_intersect, y_intersect) + # REVISIT - might not need these dist calcs? + # dist_start = distance(move.start_pos, BED_CENTER) + # dist_end = distance(move.end_pos, BED_CENTER) + # self.debug_log("dist_start: %s", dist_start) + # self.debug_log("dist_end: %s", dist_end) + # self.debug_log("zero_crossing_radius: %s", self.zero_crossing_radius) + # self.debug_log( + # "start_is_small: %s", + # dist_start < self.zero_crossing_radius, + # ) + # self.debug_log( + # "end_is_small: %s", + # dist_end < self.zero_crossing_radius, + # ) + # if ( + # dist_start <= self.zero_crossing_radius + # and dist_end <= self.zero_crossing_radius + # ): + # if ( + # move.start_pos[2] - move.end_pos[2] != 0 + # or move.start_pos[3] - move.end_pos[3] != 0 + # ): + # self.debug_log("extrude or z only move inside origin.") + # return [ + # ( + # ( + # move.start_pos[0], + # move.start_pos[1], + # move.start_pos[2], + # move.start_pos[3], + # ), + # ( + # move.start_pos[0], + # move.start_pos[1], + # move.end_pos[2], + # move.end_pos[3], + # ), + # ), + # ] + # else: + # return None + midpoint = ( + (cart_start_x + cart_end_x) / 2, + (cart_start_y + cart_end_y) / 2, + ) + + use_min = False + if ( + (cart_start_x <= closest_to_origin[0] <= cart_end_x) + or (cart_start_x >= closest_to_origin[0] >= cart_end_x) + ) and ( + (cart_start_y <= closest_to_origin[1] <= cart_end_y) + or (cart_start_y >= closest_to_origin[1] >= cart_end_y) + ): + use_min = True + zero_cross_radius = self.zero_crossing_radius + # if ( + # abs(cart_start_x) <= zero_cross_radius + # and abs(cart_start_y) <= zero_cross_radius + # ): + # zero_cross_radius = ( + # distance((cart_start_x, cart_start_y), (0, 0)) - EPSILON + # ) + # self.debug_log("zero_cross_radius: %s" % zero_cross_radius) + segmentation_radii = self.segmentation_radii + + smallest_segmentation_index = len(segmentation_radii) - 1 + # segmentation thresholds are sorted by distance, descending + if use_min: + mid_point = closest_to_origin + else: + mid_point = midpoint + + # find which thresholds our start, mid, and end are in + ( + start_circle_index, + mid_circle_index, + end_circle_index, + ) = get_region_indices( + move.start_pos, mid_point, move.end_pos, segmentation_radii + ) + self.debug_log("start_circle_index: %s", start_circle_index) + self.debug_log("mid_circle_index: %s", mid_circle_index) + self.debug_log("end_circle_index: %s", end_circle_index) + + # old code for finding which thresholds move components are in + # for index, radius in enumerate(segmentation_radii): + # #finds which thresholds our start, mid, and end are in + # if ( + # ( + # dist_start > radius + # or abs(dist_start - radius) < EPSILON + # ) + # and start_circle_index is None + # or dist_start == 0.0 + # ): + # start_circle_index = index + # if ( + # ( + # dist_end > radius + # or abs(dist_end - radius) < EPSILON + # ) + # and end_circle_index is None + # or dist_end == 0.0 + # ): + # end_circle_index = index + # if use_min: + # if ( + # ( + # dist_min > radius + # or abs(dist_min - radius) < EPSILON + # ) + # and mid_circle_index is None + # or dist_min == 0.0 + # ): + # mid_circle_index = index + # else: + # if ( + # ( + # dist_midpoint > radius + # or abs(dist_midpoint - radius) < EPSILON + # ) + # and mid_circle_index is None + # or dist_midpoint == 0.0 + # ): + # mid_circle_index = index + + self.debug_log("segmentation_radii: %s", segmentation_radii) + self.debug_log( + "start_radius: %s", segmentation_radii[start_circle_index] + ) + self.debug_log("mid_radius: %s", segmentation_radii[mid_circle_index]) + self.debug_log("end_radius: %s", segmentation_radii[end_circle_index]) + if ( + start_circle_index == mid_circle_index == end_circle_index + ) and start_circle_index != smallest_segmentation_index: + # if we don't cross a segmentation threshold + # and we're not at the smallest threshold + # then no need for segmentation! + end_pos = move.end_pos + self.debug_log("not crossing a threshold.") + # if distance(move.end_pos, (0, 0)) < self.zero_crossing_radius: + # self.debug_log("trying to move to 0! get outta here.") + # intersections = get_circle_line_intersections( + # move.start_pos, move.end_pos, self.zero_crossing_radius + # ) + # if len(intersections) == 0: + # logging.error("no intersections found!") + # self.debug_log("intersections: %s", intersections) + # self.debug_log("new end pos: %s", move.end_pos) + # end_pos = ( + # intersections[-1][0], + # intersections[-1][1], + # move.end_pos[2], + # move.end_pos[3], + # ) + end_pos = ( + round(end_pos[0], ROUND_CONSTANT), + round(end_pos[1], ROUND_CONSTANT), + round(end_pos[2], ROUND_CONSTANT), + round(end_pos[3], ROUND_CONSTANT), + ) + start_pos = ( + round(move.start_pos[0], ROUND_CONSTANT), + round(move.start_pos[1], ROUND_CONSTANT), + round(move.start_pos[2], ROUND_CONSTANT), + round(move.start_pos[3], ROUND_CONSTANT), + ) + if end_pos == start_pos: + self.debug_log("end_pos == start_pos") + return None + return ((start_pos, end_pos),) + + self.debug_log("start_circle_index: %s", start_circle_index) + self.debug_log("segmentation_radii: %s", segmentation_radii) + + intersections = OrderedDict() + indices_to_traverse = [] + if start_circle_index >= mid_circle_index >= end_circle_index: + # 8 5 2 for example + # moving from inside to outside + indices_to_traverse = list( + range(start_circle_index, end_circle_index - 1, -1) + ) + elif start_circle_index <= mid_circle_index <= end_circle_index: + # 2 5 8 for example + # moving from outside to inside + indices_to_traverse = list( + range(start_circle_index, end_circle_index + 1) + ) + elif mid_circle_index >= start_circle_index: + indices_to_traverse = [] + # 4 8 2 for example. + # moving past center, further from inside than outside + for i in range(start_circle_index, mid_circle_index + 1): + indices_to_traverse.append(i) + for i in range(mid_circle_index, end_circle_index - 1, -1): + indices_to_traverse.append(i) + # we can dedupe because we an intersection calc will get both intersection points if 2 exist + indices_to_traverse = list(set(indices_to_traverse)) + ending_within_zero_radius = False + if end_circle_index == smallest_segmentation_index: + # if we're ending in the zero radius *and* it's less than the zero crossing radius + # aka, trying to move to a position with a dist of 0 < dist < zero_crossing_radius + if distance(move.end_pos, (0, 0)) < self.zero_crossing_radius: + ending_within_zero_radius = True + handled_zero = False + self.debug_log("indices_to_traverse: %s", indices_to_traverse) + self.debug_log("intersections: %s", intersections) + for i in indices_to_traverse: + radius = segmentation_radii[i] + + if radius not in intersections: + intersection_subset = get_circle_line_intersections( + move.start_pos, move.end_pos, radius + ) + self.debug_log("intersection_subset: %s", intersection_subset) + if i == len(segmentation_radii) - 1 and not handled_zero: + # we're in the zero radius + # print("zero radius!") + + if len(intersection_subset) == 2: + + # print("zero crossing with two intersections") + # moving through our zero radius, move 90 deg to incoming line + # if we know we're zero crossing, the angle to start and to end will always be pi (180deg) apart + # we want to find a point perpendicular to the line between start and end, at a distance of self.zero_crossing_radius + incoming_angle = math.atan2( + intersection_subset[0][1], + intersection_subset[0][0], + ) + perpendicular_angle = incoming_angle + (math.pi / 2) + offset_position = polar_to_cartesian( + self.zero_crossing_radius, perpendicular_angle + ) + intersections[radius] = (offset_position,) + elif len(intersection_subset) == 1: + # moving to or from zero radius + + intersections[radius] = (intersection_subset[0],) + else: + pass + # somehow we found ourselves in the lowest radius, but we don't intersect with it? + self.debug_log( + "looking at smallest radius, but no intersection" + ) + self.debug_log(intersection_subset) + # return [(move.start_pos, move.end_pos)] + handled_zero = True + continue + if len(intersection_subset): + intersections[radius] = intersection_subset + + # intersections is an ordered dict, descending, + # radius -> [closest_intersection, furthest_intersection] + # we traverse by radius. if there are two intersections, + # grab [0] and put it at the end of a start list, then + # grab [1] and put it at the front of an end list + # if there's only one, put it to the end of start + # at the end, we'll put start + end together for the full list of points + # flatten values of intersections + flattened_intersections = [] + for radius, intersection_subset in intersections.items(): + flattened_intersections.extend(intersection_subset) + total_intersections = flattened_intersections + total_intersections = sorted( + total_intersections, + key=lambda x: sqrdistance(x, move.start_pos), + ) + if ( + # if start pos is basically the same as the first intersection, + # start from start pos instead + abs(move.start_pos[0] - total_intersections[0][0]) < EPSILON + and abs(move.start_pos[1] - total_intersections[0][1]) < EPSILON + ): + total_intersections = [move.start_pos] + total_intersections[1:] + else: # else, start at start, then go to first intersection + total_intersections = [move.start_pos] + total_intersections + if ending_within_zero_radius: + pass + self.debug_log( + "ending within zero radius, total_intersections: %s", + total_intersections, + ) + elif ( + abs(move.end_pos[0] - total_intersections[-1][0]) < EPSILON + and abs(move.end_pos[1] - total_intersections[-1][1]) < EPSILON + ): # else, end at last intersection, then go to end + total_intersections = total_intersections[:-1] + [move.end_pos] + else: + total_intersections = total_intersections + [move.end_pos] + + self.debug_log("total_intersections: %s", total_intersections) + # sort total intersections by distance from start + # if move.end_pos[0] == 0 and move.end_pos[1] == 0: + # total_intersections.pop(-1) + if move.start_pos[0] == 0 and move.start_pos[1] == 0: + total_intersections.pop(0) + xy_moves = [] + while len(total_intersections) != 1: + start = total_intersections.pop(0) + end = total_intersections[0] + xy_moves.append((start, end)) + total_move_dist = distance(move.start_pos, move.end_pos) + total_z_dist = move.end_pos[2] - move.start_pos[2] + total_e_dist = move.end_pos[3] - move.start_pos[3] + actual_moves = [] + current_z_pos = move.start_pos[2] + current_e_pos = move.start_pos[3] + for move in xy_moves: + move_dist = distance(move[0], move[1]) + z_dist = move_dist / total_move_dist * total_z_dist + e_dist = move_dist / total_move_dist * total_e_dist + new_z_pos = current_z_pos + z_dist + new_e_pos = current_e_pos + e_dist + actual_moves.append( + ( + ( + round(move[0][0], 14), + round(move[0][1], 14), + round(current_z_pos, 14), + round(current_e_pos, 14), + ), + ( + round(move[1][0], 14), + round(move[1][1], 14), + round(new_z_pos, 14), + round(new_e_pos, 14), + ), + ) + ) + current_e_pos = new_e_pos + current_z_pos = new_z_pos + self.debug_log("actual moves: %s", actual_moves) + # if we had a single intersection, + # that is an edge case where we are trying to move + # within the zero crossing radius + # from the edge of the zero crossing radius. + if len(actual_moves) == 0 and len(total_intersections) == 1: + if move.axes_d[2] != 0 and move.axes_d[3] != 0: + return None + else: + intersection = total_intersections[0] + return [(move.start_pos, [intersection[0], intersection[1], move.end_pos[2], move.end_pos[3]])] + return actual_moves + else: + return [] + + def get_status(self, eventtime): + xy_home = "xy" if self.limit_xy2 >= 0.0 else "" + z_home = "z" if self.limit_z[0] <= self.limit_z[1] else "" + return { + "homed_axes": xy_home + z_home, + "axis_minimum": self.axes_min, + "axis_maximum": self.axes_max, + } + + +def load_kinematics(toolhead, config): + return PolarXZKinematics(toolhead, config) diff --git a/klippy/stepper.py b/klippy/stepper.py index 0a79f88ea..8e3245fff 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -328,7 +328,12 @@ def PrinterStepper(config, units_in_radians=False): units_in_radians, ) # Register with helper modules - for mname in ["stepper_enable", "force_move", "motion_report"]: + for mname in [ + "stepper_enable", + "force_move", + "motion_report", + "polar_alignment", + ]: m = printer.load_object(config, mname) m.register_stepper(config, mcu_stepper) return mcu_stepper diff --git a/klippy/toolhead.py b/klippy/toolhead.py index d38eda6fa..c42da904e 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -4,6 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging, importlib +import mcu import chelper import kinematics.extruder @@ -15,6 +16,8 @@ # Class to track each move request class Move: def __init__(self, toolhead, start_pos, end_pos, speed): + print("start_pos", start_pos) + print("end_pos", end_pos) self.toolhead = toolhead self.start_pos = tuple(start_pos) self.end_pos = tuple(end_pos) @@ -23,8 +26,12 @@ def __init__(self, toolhead, start_pos, end_pos, speed): self.timing_callbacks = [] velocity = min(speed, toolhead.max_velocity) self.is_kinematic_move = True - self.axes_d = axes_d = [end_pos[i] - start_pos[i] for i in (0, 1, 2, 3)] - self.move_d = move_d = math.sqrt(sum([d * d for d in axes_d[:3]])) + self.axes_d = axes_d = [ + end_pos[i] - start_pos[i] for i in (0, 1, 2, 3) + ] # distance of each axis + self.move_d = move_d = math.sqrt( + sum([d * d for d in axes_d[:3]]) + ) # total move distance if move_d < 0.000000001: # Extrude only move self.end_pos = ( @@ -44,6 +51,8 @@ def __init__(self, toolhead, start_pos, end_pos, speed): else: inv_move_d = 1.0 / move_d self.axes_r = [d * inv_move_d for d in axes_d] + # logging.info('axes_d', self.axes_d) + # logging.info('axes_r', self.axes_r) self.min_move_t = move_d / velocity # Junction speeds are tracked in velocity squared. The # delta_v2 is the maximum amount of this squared-velocity that @@ -122,9 +131,19 @@ def set_junction(self, start_v2, cruise_v2, end_v2): self.end_v = end_v = math.sqrt(end_v2) # Determine time spent in each portion of move (time is the # distance divided by average velocity) - self.accel_t = accel_d / ((start_v + cruise_v) * 0.5) - self.cruise_t = cruise_d / cruise_v - self.decel_t = decel_d / ((end_v + cruise_v) * 0.5) + # logging.info("start_v: %f, cruise_v: %f, end_v: %f", start_v, cruise_v, end_v) + if (start_v + cruise_v) == 0: + self.accel_t = 0 + else: + self.accel_t = accel_d / ((start_v + cruise_v) * 0.5) + if cruise_v == 0: + self.cruise_t = 0 + else: + self.cruise_t = cruise_d / cruise_v + if end_v + cruise_v == 0: + self.decel_t = 0 + else: + self.decel_t = decel_d / ((end_v + cruise_v) * 0.5) LOOKAHEAD_FLUSH_TIME = 0.250 @@ -164,6 +183,7 @@ def flush(self, lazy=False): move = queue[i] reachable_start_v2 = next_end_v2 + move.delta_v2 start_v2 = min(move.max_start_v2, reachable_start_v2) + reachable_smoothed_v2 = next_smoothed_v2 + move.smooth_delta_v2 smoothed_v2 = min(move.max_smoothed_v2, reachable_smoothed_v2) if smoothed_v2 < reachable_smoothed_v2: @@ -186,6 +206,9 @@ def flush(self, lazy=False): if not update_flush_count and i < flush_count: mc_v2 = peak_cruise_v2 for m, ms_v2, me_v2 in reversed(delayed): + # logging.info("cruise_v2: %f", mc_v2) + # logging.info("start_v2: %f", ms_v2) + # logging.info("m: %f", m.__dict__) mc_v2 = min(mc_v2, ms_v2) m.set_junction( min(ms_v2, mc_v2), mc_v2, min(me_v2, mc_v2) @@ -221,6 +244,7 @@ def add_move(self, move): move.calc_junction(self.queue[-2]) self.junction_flush -= move.min_move_t if self.junction_flush <= 0.0: + # logging.info('flushing...') # Enough moves have been queued to reach the target flush time. self.flush(lazy=True) @@ -477,6 +501,7 @@ def _check_pause(self): if not self.can_pause: self.need_check_pause = self.reactor.NEVER return + logging.info("stalling...") eventtime = self.reactor.pause(eventtime + min(1.0, pause_time)) est_print_time = self.mcu.estimated_print_time(eventtime) buffer_time = self.print_time - est_print_time @@ -540,15 +565,29 @@ def set_position(self, newpos, homing_axes=()): self.printer.send_event("toolhead:set_position") def move(self, newpos, speed): + # logging.info('commanded_pos: %s', self.commanded_pos) + # logging.info('newpos: %s', newpos) move = Move(self, self.commanded_pos, newpos, speed) if not move.move_d: return - if move.is_kinematic_move: - self.kin.check_move(move) - if move.axes_d[3]: - self.extruder.check_move(move) - self.commanded_pos[:] = move.end_pos - self.move_queue.add_move(move) + moves = [move] + if hasattr(self.kin, "segment_move"): + move_positions = self.kin.segment_move(move) + if move_positions is None: + moves = [] + elif move_positions: + # logging.info("move_positions: %s", move_positions) + moves = [ + Move(self, move[0], move[1], speed) + for move in move_positions + ] + for move in moves: + if move.is_kinematic_move: + self.kin.check_move(move) + if move.axes_d[3]: + self.extruder.check_move(move) + self.commanded_pos[:] = move.end_pos + self.move_queue.add_move(move) if self.print_time > self.need_check_pause: self._check_pause() @@ -615,6 +654,7 @@ def drip_move(self, newpos, speed, drip_completion): self.drip_completion = drip_completion # Submit move try: + # logging.info('drip newpos: %s', newpos) self.move(newpos, speed) except self.printer.command_error as e: self.reactor.update_timer(self.flush_timer, self.reactor.NOW) @@ -737,6 +777,12 @@ def cmd_SET_VELOCITY_LIMIT(self, gcmd): requested_accel_to_decel = gcmd.get_float( "ACCEL_TO_DECEL", None, above=0.0 ) + requested_rotational_accel = gcmd.get_float( + "ROTATIONAL_ACCEL", None, above=0.0 + ) + requested_rotational_velocity = gcmd.get_float( + "ROTATIONAL_VELOCITY", None, above=0.0 + ) if max_velocity is not None: self.max_velocity = max_velocity if max_accel is not None: @@ -745,6 +791,12 @@ def cmd_SET_VELOCITY_LIMIT(self, gcmd): self.square_corner_velocity = square_corner_velocity if requested_accel_to_decel is not None: self.requested_accel_to_decel = requested_accel_to_decel + if requested_rotational_accel is not None: + self.kin.max_rotational_accel = requested_rotational_accel + if requested_rotational_velocity is not None: + self.kin.max_rotational_velocity = math.radians( + requested_rotational_velocity + ) self._calc_junction_deviation() msg = ( "max_velocity: %.6f\n" diff --git a/klippy/util.py b/klippy/util.py index 1bf0190f4..e40f493c9 100644 --- a/klippy/util.py +++ b/klippy/util.py @@ -12,6 +12,7 @@ # Low-level Unix commands ###################################################################### + # Return the SIGINT interrupt handler back to the OS default def fix_sigint(): signal.signal(signal.SIGINT, signal.SIG_DFL) @@ -19,6 +20,7 @@ def fix_sigint(): fix_sigint() + # Set a file-descriptor as non-blocking def set_nonblock(fd): fcntl.fcntl( diff --git a/scripts/klipper_git_reset.sh b/scripts/klipper_git_reset.sh new file mode 100644 index 000000000..e69de29bb diff --git a/test/klippy/polarxz.test b/test/klippy/polarxz.test new file mode 100644 index 000000000..413d0ec44 --- /dev/null +++ b/test/klippy/polarxz.test @@ -0,0 +1,74 @@ +# Test case for basic movement on polar printers +CONFIG ../../config/example-polarxz.cfg +DICTIONARY atmega2560.dict + +; Start by homing the printer. +G28 +G90 +G1 F6000 + +; Z / X / Y moves +G1 Z1 +G1 X1 +G1 Y1 + +; Delayed moves +G1 Y2 +G4 P100 +G1 Y1.5 +M400 +G1 Y1 + +; diagonal moves +G1 X10 Y0 +G1 X1 Z2 +G1 X0 Y1 Z1 + +; extrude only moves +G1 E1 +G1 E0 + +; regular extrude move +G1 X10 Y0 E.01 + +; Multiple rotations +g1 X10 Y10 +g1 X-10 Y10 +g1 X-10 Y-10 +g1 X10 Y-10 + +g1 X10 Y15 +g1 X-10 Y15 +g1 X-10 Y-15 +g1 X10 Y-15 + +g1 X10 Y20 +g1 X-10 Y20 +g1 X-10 Y-20 +g1 X10 Y-20 + +g1 X10 Y25 +g1 X-10 Y25 +g1 X-10 Y-25 +g1 X10 Y-25 + +; Multiple rotations in reverse direction +g1 X-15 Y-25 +g1 X-15 Y25 +g1 X15 Y25 +g1 X15 Y-25 + +g1 X-20 Y-25 +g1 X-20 Y25 +g1 X20 Y25 +g1 X20 Y-25 + +g1 X-25 Y-25 +g1 X-25 Y25 +g1 X25 Y25 +g1 X25 Y-25 + +g1 X-30 Y-25 +g1 X-30 Y25 +g1 X30 Y25 +g1 X30 Y-25