diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index afd4d922..22649f72 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -61,7 +61,7 @@ defs_trapq = """ void trapq_append(struct trapq *tq, double print_time , double accel_t, double cruise_t, double decel_t , double start_pos_x, double start_pos_y, double start_pos_z - , double axes_d_x, double axes_d_y, double axes_d_z + , double axes_r_x, double axes_r_y, double axes_r_z , double start_v, double cruise_v, double accel); struct trapq *trapq_alloc(void); void trapq_free(struct trapq *tq); diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index 851a15b7..4c2356ad 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -25,19 +25,11 @@ void __visible trapq_append(struct trapq *tq, double print_time , double accel_t, double cruise_t, double decel_t , double start_pos_x, double start_pos_y, double start_pos_z - , double axes_d_x, double axes_d_y, double axes_d_z + , double axes_r_x, double axes_r_y, double axes_r_z , double start_v, double cruise_v, double accel) { - struct coord axes_r, start_pos; - double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y - + axes_d_z*axes_d_z); - axes_r.x = axes_d_x * inv_move_d; - axes_r.y = axes_d_y * inv_move_d; - axes_r.z = axes_d_z * inv_move_d; - start_pos.x = start_pos_x; - start_pos.y = start_pos_y; - start_pos.z = start_pos_z; - + struct coord start_pos = { x: start_pos_x, y: start_pos_y, z: start_pos_z }; + struct coord axes_r = { x: axes_r_x, y: axes_r_y, z: axes_r_z }; if (accel_t) { struct move *m = move_alloc(); m->print_time = print_time; diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index 787d83a4..ec762621 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -23,7 +23,7 @@ struct move *move_alloc(void); void trapq_append(struct trapq *tq, double print_time , double accel_t, double cruise_t, double decel_t , double start_pos_x, double start_pos_y, double start_pos_z - , double axes_d_x, double axes_d_y, double axes_d_z + , double axes_r_x, double axes_r_y, double axes_r_z , double start_v, double cruise_v, double accel); double move_get_distance(struct move *m, double move_time); struct coord move_get_coord(struct move *m, double move_time); diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index fe3b3a60..2f748c0a 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -11,16 +11,19 @@ STALL_TIME = 0.100 # Calculate a move's accel_t, cruise_t, and cruise_v def calc_move_time(dist, speed, accel): - dist = abs(dist) + axis_r = 1. + if dist < 0.: + axis_r = -1. + dist = -dist if not accel or not dist: - return 0., dist / speed, speed + return axis_r, 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 accel_t, cruise_t, speed + return axis_r, accel_t, cruise_t, speed class ForceMove: def __init__(self, config): @@ -67,9 +70,9 @@ class ForceMove: print_time = toolhead.get_last_move_time() prev_sk = stepper.set_stepper_kinematics(self.stepper_kinematics) stepper.set_position((0., 0., 0.)) - accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel) + axis_r, accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel) self.trapq_append(self.trapq, print_time, accel_t, cruise_t, accel_t, - 0., 0., 0., dist, 0., 0., 0., cruise_v, accel) + 0., 0., 0., axis_r, 0., 0., 0., cruise_v, accel) print_time += accel_t + cruise_t + accel_t stepper.generate_steps(print_time) self.trapq_free_moves(self.trapq, print_time) diff --git a/klippy/extras/manual_stepper.py b/klippy/extras/manual_stepper.py index 9b985007..73766622 100644 --- a/klippy/extras/manual_stepper.py +++ b/klippy/extras/manual_stepper.py @@ -54,11 +54,11 @@ class ManualStepper: self.sync_print_time() cp = self.stepper.get_commanded_position() dist = movepos - cp - accel_t, cruise_t, cruise_v = force_move.calc_move_time( + 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., dist, 0., 0., + cp, 0., 0., axis_r, 0., 0., 0., cruise_v, accel) self.next_cmd_time += accel_t + cruise_t + accel_t self.stepper.generate_steps(self.next_cmd_time) diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 6b9245d4..7d702280 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -90,7 +90,7 @@ class PrinterExtruder: def motor_off(self, print_time): self.stepper.motor_enable(print_time, 0) def check_move(self, move): - move.extrude_r = move.axes_d[3] / move.move_d + move.extrude_r = move.axes_r[3] move.extrude_max_corner_v = 0. if not self.heater.can_extrude: raise homing.EndstopError( @@ -111,7 +111,7 @@ class PrinterExtruder: # Permit extrusion if amount extruded is tiny move.extrude_r = self.max_extrude_ratio return - area = move.axes_d[3] * self.filament_area / move.move_d + area = move.axes_r[3] * self.filament_area logging.debug("Overextrude: %s vs %s (area=%.3f dist=%.3f)", move.extrude_r, self.max_extrude_ratio, area, move.move_d) @@ -172,7 +172,7 @@ class PrinterExtruder: return flush_count def move(self, print_time, move): axis_d = move.axes_d[3] - axis_r = axis_d / move.move_d + axis_r = move.axes_r[3] accel = move.accel * axis_r start_v = move.start_v * axis_r cruise_v = move.cruise_v * axis_r diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 247add5a..8fdcf1e9 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -27,9 +27,15 @@ class Move: end_pos[3]) axes_d[0] = axes_d[1] = axes_d[2] = 0. self.move_d = move_d = abs(axes_d[3]) + inv_move_d = 0. + if move_d: + inv_move_d = 1. / move_d self.accel = 99999999.9 velocity = speed self.is_kinematic_move = False + else: + inv_move_d = 1. / move_d + self.axes_r = [d * inv_move_d for d in axes_d] 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 @@ -53,12 +59,11 @@ class Move: # Allow extruder to calculate its maximum junction extruder_v2 = self.toolhead.extruder.calc_junction(prev_move, self) # Find max velocity using "approximated centripetal velocity" - axes_d = self.axes_d - prev_axes_d = prev_move.axes_d - junction_cos_theta = -((axes_d[0] * prev_axes_d[0] - + axes_d[1] * prev_axes_d[1] - + axes_d[2] * prev_axes_d[2]) - / (self.move_d * prev_move.move_d)) + axes_r = self.axes_r + prev_axes_r = prev_move.axes_r + junction_cos_theta = -(axes_r[0] * prev_axes_r[0] + + axes_r[1] * prev_axes_r[1] + + axes_r[2] * prev_axes_r[2]) if junction_cos_theta > 0.999999: return junction_cos_theta = max(junction_cos_theta, -0.999999) @@ -302,7 +307,7 @@ class ToolHead: self.trapq, next_move_time, move.accel_t, move.cruise_t, move.decel_t, move.start_pos[0], move.start_pos[1], move.start_pos[2], - move.axes_d[0], move.axes_d[1], move.axes_d[2], + move.axes_r[0], move.axes_r[1], move.axes_r[2], move.start_v, move.cruise_v, move.accel) if move.axes_d[3]: self.extruder.move(next_move_time, move)