Skip to content

Commit

Permalink
changes for DK actions
Browse files Browse the repository at this point in the history
  • Loading branch information
@bwnance
bwnance committed Jan 7, 2024
1 parent 4ea507d commit 9d7ad5f
Show file tree
Hide file tree
Showing 9 changed files with 192 additions and 281 deletions.
2 changes: 1 addition & 1 deletion klippy/chelper/itersolve.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +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)
Expand Down
2 changes: 1 addition & 1 deletion klippy/extras/bed_mesh.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -425,7 +425,7 @@ def _generate_points(self, error):
pos_y = min_y
points = []

if self.polar == True:
if self.polar is True:
radius_points = x_cnt // 2
radius = self.radius
spacing = radius // radius_points
Expand Down
151 changes: 0 additions & 151 deletions klippy/extras/force_move.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,157 +60,6 @@ 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()
Expand Down
80 changes: 50 additions & 30 deletions klippy/extras/homeable_stepper.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,13 @@
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
from toolhead import (
DripModeEndSignal,
DRIP_SEGMENT_TIME,
DRIP_TIME,
SDS_CHECK_TIME,
)


class HomeableStepper(MCU_stepper):
def __init__(self, config, *args, **kwargs):
Expand All @@ -20,12 +26,15 @@ def __init__(self, config, *args, **kwargs):
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.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')]
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")
Expand All @@ -40,7 +49,7 @@ def setup_itersolve_for_homing(self):
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("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:
Expand All @@ -67,7 +76,9 @@ def do_enable(self, enable):
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):
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()
Expand Down Expand Up @@ -97,9 +108,13 @@ def do_manual_move(self, movepos, speed, accel, sync=True, drip_completion=None)
if drip_completion is not None:
logging.info("drip start...")
next_cmd_time = self.next_cmd_time
toolhead = self.printer.lookup_object("toolhead")
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)
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
Expand All @@ -111,28 +126,29 @@ def do_manual_move(self, movepos, speed, accel, sync=True, drip_completion=None)
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:
if wait_time > 0.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)
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)
Expand All @@ -143,33 +159,35 @@ def do_manual_move(self, movepos, speed, accel, sync=True, drip_completion=None)
self.sync_print_time()

def home(self, accel):
#homing dist is one full rotation
# homing dist is one full rotation
self.homing_accel = accel
logging.info("accel: %s", accel)
homing_dist = 3*math.pi
homing_dist = 3 * math.pi
if self.homing_positive_dir:
homing_dir = 1
else:
homing_dir = -1
#homing is homing_dir
homing_dir = -1
# homing is homing_dir
homing_dist = homing_dist * homing_dir
#retract dir is opposite of homing dir
# retract dir is opposite of homing dir
homing_retract_dist = self.homing_retract_dist * homing_dir * -1
#set position to 0
# set position to 0
self.do_set_position(0.0)
#primary homing move
# primary homing move
self.do_homing_move(
homing_dist,
self.homing_speed,
self.homing_accel,
True,
True,
)
#retract move
# 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_manual_move(
movepos, self.homing_retract_speed, self.homing_accel
)
self.do_set_position(0.0)
self.do_homing_move(
homing_dist,
Expand All @@ -178,22 +196,18 @@ def home(self, accel):
True,
True,
)
#need to move to actual 0, and we are at position_endstop

# 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"
)
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")
Expand Down Expand Up @@ -259,9 +273,8 @@ def get_position(self):
# 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]
pos = [self.get_commanded_position(), 0.0, 0.0]
logging.info("homeable_stepper pos: %s", pos)
return pos

Expand All @@ -278,10 +291,17 @@ def dwell(self, 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)
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

Expand Down
2 changes: 2 additions & 0 deletions klippy/extras/manual_probe.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import bisect
import logging


class ManualProbe:
Expand Down Expand Up @@ -153,6 +154,7 @@ def verify_no_manual_probe(printer):
Z_BOB_MINIMUM = 0.500
BISECT_MAX = 0.200


# Helper script to determine a Z height
class ManualProbeHelper:
def __init__(self, printer, gcmd, finalize_callback):
Expand Down
Loading

0 comments on commit 9d7ad5f

Please sign in to comment.