mirror of
https://github.com/Klipper3d/klipper.git
synced 2026-01-06 14:57:53 -07:00
Añadir extras de resonance_test y zmax_homing
This commit is contained in:
microlay
parent
9346ad1914
commit
20c2f1654c
2 changed files with 1286 additions and 0 deletions
455
klippy/extras/resonance_test.py
Normal file
455
klippy/extras/resonance_test.py
Normal file
|
|
@ -0,0 +1,455 @@
|
|||
# Archivo: klipper/klippy/extras/resonance_test.py
|
||||
|
||||
# Copyright (C) 2024 MicroLay
|
||||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
|
||||
import logging, math
|
||||
|
||||
class TestAxis:
|
||||
def __init__(self, axis='z'):
|
||||
self._name = axis
|
||||
self._vib_dir = (0., 0., 1.) if axis == 'z' else (0., 0., 0.)
|
||||
|
||||
def get_name(self):
|
||||
return self._name
|
||||
|
||||
def get_point(self, l):
|
||||
return (0., 0., self._vib_dir[2] * l)
|
||||
|
||||
class VibrationGenerator:
|
||||
def __init__(self, config):
|
||||
self.min_freq = config.getfloat('min_freq', 5., minval=1.)
|
||||
self.max_freq = config.getfloat('max_freq', 135., minval=self.min_freq, maxval=100000.)
|
||||
self.accel_per_hz = config.getfloat('accel_per_hz', 60., above=0.)
|
||||
self.hz_per_sec = config.getfloat('hz_per_sec', 1., minval=0.1, maxval=100.)
|
||||
self.default_amplitude = config.getfloat('amplitude', 1., above=0.) # Amplitud en mm
|
||||
|
||||
def prepare_test(self, gcmd):
|
||||
self.freq_start = gcmd.get_float("FREQ_START", self.min_freq, minval=1.)
|
||||
self.freq_end = gcmd.get_float("FREQ_END", self.max_freq, minval=self.freq_start, maxval=100000.)
|
||||
self.test_amplitude = gcmd.get_float("AMPLITUDE", self.default_amplitude, above=0.)
|
||||
self.test_hz_per_sec = gcmd.get_float("HZ_PER_SEC", self.hz_per_sec, above=0., maxval=100.)
|
||||
|
||||
# Calcular aceleración basada en la amplitud deseada
|
||||
# Para un movimiento sinusoidal: a = (2πf)²A
|
||||
# donde f es la frecuencia y A es la amplitud
|
||||
self.test_accel_per_hz = (2. * math.pi) ** 2 * self.test_amplitude
|
||||
|
||||
def gen_test(self):
|
||||
freq = self.freq_start
|
||||
res = []
|
||||
sign = 1.
|
||||
time = 0.
|
||||
# Asegurar un incremento mínimo para evitar quedarse atascado
|
||||
min_freq_increment = 0.01 # Incremento mínimo de 0.01 Hz
|
||||
|
||||
# Caso especial: si freq_start == freq_end, generar al menos un ciclo
|
||||
if abs(self.freq_start - self.freq_end) < 0.00001:
|
||||
t_seg = .25 / freq
|
||||
accel = self.test_accel_per_hz * freq * freq # a = (2πf)²A
|
||||
|
||||
# Generar al menos 4 puntos (un ciclo completo)
|
||||
time += t_seg
|
||||
res.append((time, sign * accel, freq))
|
||||
time += t_seg
|
||||
res.append((time, -sign * accel, freq))
|
||||
time += t_seg
|
||||
res.append((time, -sign * accel, freq))
|
||||
time += t_seg
|
||||
res.append((time, sign * accel, freq))
|
||||
return res
|
||||
|
||||
# Usar una condición más estricta para asegurar la terminación
|
||||
while freq < self.freq_end:
|
||||
t_seg = .25 / freq
|
||||
accel = self.test_accel_per_hz * freq * freq # a = (2πf)²A
|
||||
time += t_seg
|
||||
res.append((time, sign * accel, freq))
|
||||
time += t_seg
|
||||
res.append((time, -sign * accel, freq))
|
||||
|
||||
# Calcular el incremento basado en el algoritmo original
|
||||
freq_increment = 2. * t_seg * self.test_hz_per_sec
|
||||
|
||||
# Asegurar que el incremento no sea demasiado pequeño
|
||||
if freq_increment < min_freq_increment:
|
||||
freq_increment = min_freq_increment
|
||||
|
||||
# Prevenir incrementos demasiado grandes para frecuencias muy bajas
|
||||
if freq_increment > (self.freq_end - self.freq_start) / 10.0:
|
||||
freq_increment = (self.freq_end - self.freq_start) / 10.0
|
||||
|
||||
freq += freq_increment
|
||||
|
||||
# Asegurar que la última iteración llegue exactamente a freq_end
|
||||
if freq > self.freq_end - freq_increment and freq < self.freq_end:
|
||||
freq = self.freq_end
|
||||
|
||||
sign = -sign
|
||||
|
||||
# Añadir la frecuencia final si no está ya
|
||||
if len(res) > 0 and res[-1][2] < self.freq_end:
|
||||
t_seg = .25 / self.freq_end
|
||||
accel = self.test_accel_per_hz * self.freq_end * self.freq_end
|
||||
time += t_seg
|
||||
res.append((time, sign * accel, self.freq_end))
|
||||
time += t_seg
|
||||
res.append((time, -sign * accel, self.freq_end))
|
||||
|
||||
return res
|
||||
|
||||
class ResonanceTest:
|
||||
def __init__(self, config):
|
||||
self.printer = config.get_printer()
|
||||
self.gcode = self.printer.lookup_object('gcode')
|
||||
self.axis = TestAxis('z')
|
||||
self.generator = VibrationGenerator(config)
|
||||
|
||||
# Registrar comandos
|
||||
self.gcode.register_command(
|
||||
'RESONANCE_TEST_START',
|
||||
self.cmd_RESONANCE_TEST_START,
|
||||
desc=self.cmd_RESONANCE_TEST_START_help
|
||||
)
|
||||
self.gcode.register_command(
|
||||
'RESONANCE_TEST_STOP',
|
||||
self.cmd_RESONANCE_TEST_STOP,
|
||||
desc=self.cmd_RESONANCE_TEST_STOP_help
|
||||
)
|
||||
self.gcode.register_command(
|
||||
'RESONANCE_TEST_FIXED',
|
||||
self.cmd_RESONANCE_TEST_FIXED,
|
||||
desc=self.cmd_RESONANCE_TEST_FIXED_help
|
||||
)
|
||||
|
||||
# Variables para el control de la prueba
|
||||
self.is_testing = False
|
||||
|
||||
def _check_axis_homed(self, toolhead, axis, skip_home):
|
||||
status = toolhead.get_status(self.printer.get_reactor().monotonic())
|
||||
homed_axes = status.get('homed_axes', '')
|
||||
if axis in homed_axes or skip_home:
|
||||
return True
|
||||
return False
|
||||
|
||||
cmd_RESONANCE_TEST_START_help = "Inicia la prueba de resonancia con movimientos oscilatorios"
|
||||
def cmd_RESONANCE_TEST_START(self, gcmd):
|
||||
if self.is_testing:
|
||||
raise gcmd.error("Ya hay una prueba de resonancia en curso")
|
||||
|
||||
# Preparar la prueba
|
||||
self.generator.prepare_test(gcmd)
|
||||
test_seq = self.generator.gen_test()
|
||||
|
||||
# Verificar que la secuencia de prueba no esté vacía
|
||||
if not test_seq:
|
||||
raise gcmd.error("No se pudo generar una secuencia de prueba válida con los parámetros proporcionados")
|
||||
|
||||
# Obtener parámetros para controlar el homing
|
||||
no_home = gcmd.get_int("NO_HOME", 0)
|
||||
skip_pre_home = gcmd.get_int("SKIP_HOME", 0)
|
||||
|
||||
# Obtener objetos necesarios
|
||||
reactor = self.printer.get_reactor()
|
||||
toolhead = self.printer.lookup_object('toolhead')
|
||||
|
||||
# Verificar si Z está homeado, si es necesario
|
||||
if not self._check_axis_homed(toolhead, 'z', skip_pre_home):
|
||||
if not skip_pre_home:
|
||||
gcmd.respond_info("Ejecutando G28 Z antes de la prueba...")
|
||||
self.gcode.run_script_from_command("G28 Z")
|
||||
else:
|
||||
gcmd.respond_info("ADVERTENCIA: Eje Z no está homeado pero SKIP_HOME=1 fue especificado")
|
||||
|
||||
# Guardar configuración actual
|
||||
systime = reactor.monotonic()
|
||||
toolhead_info = toolhead.get_status(systime)
|
||||
old_max_accel = toolhead_info['max_accel']
|
||||
old_max_z_accel = toolhead_info.get('max_z_accel', old_max_accel)
|
||||
old_max_velocity = toolhead_info['max_velocity']
|
||||
old_max_z_velocity = toolhead_info.get('max_z_velocity', old_max_velocity)
|
||||
|
||||
# Obtener 'minimum_cruise_ratio' solo si existe, de lo contrario usar un valor predeterminado
|
||||
old_minimum_cruise_ratio = toolhead_info.get('minimum_cruise_ratio', 0.5)
|
||||
|
||||
# Calcular aceleración máxima necesaria
|
||||
max_accel = max([abs(a) for _, a, _ in test_seq]) if test_seq else old_max_accel
|
||||
|
||||
# Variables para restauración
|
||||
tmc_z = None
|
||||
old_thresh = None
|
||||
|
||||
try:
|
||||
self.is_testing = True
|
||||
|
||||
# Configurar límites de velocidad más altos para la prueba
|
||||
self.gcode.run_script_from_command(
|
||||
"SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.3f SQUARE_CORNER_VELOCITY=5 VELOCITY=500"
|
||||
% (max_accel, max_accel))
|
||||
|
||||
# Deshabilitar el modo stealthchop durante la prueba
|
||||
tmc_z = self.printer.lookup_object('tmc5160 stepper_z', None)
|
||||
if tmc_z is not None:
|
||||
try:
|
||||
old_thresh = tmc_z.get_register("TPWMTHRS")
|
||||
tmc_z.set_register("TPWMTHRS", 0)
|
||||
except:
|
||||
gcmd.respond_info("No se pudo modificar TPWMTHRS")
|
||||
|
||||
# Obtener posición actual
|
||||
X, Y, Z, E = toolhead.get_position()
|
||||
|
||||
# Ejecutar secuencia de prueba
|
||||
last_v = last_t = last_freq = 0.
|
||||
gcmd.respond_info("Iniciando prueba de resonancia de %.1f Hz a %.1f Hz" %
|
||||
(self.generator.freq_start, self.generator.freq_end))
|
||||
|
||||
# Mostrar el número total de pasos para dar feedback de progreso
|
||||
total_steps = len(test_seq)
|
||||
current_step = 0
|
||||
|
||||
for next_t, accel, freq in test_seq:
|
||||
current_step += 1
|
||||
if not self.is_testing:
|
||||
break
|
||||
|
||||
t_seg = next_t - last_t
|
||||
toolhead.cmd_M204(self.gcode.create_gcode_command(
|
||||
"M204", "M204", {"S": abs(accel)}))
|
||||
|
||||
# Calcular nueva velocidad y posición
|
||||
v = last_v + accel * t_seg
|
||||
abs_v = abs(v)
|
||||
if abs_v < 0.000001:
|
||||
v = abs_v = 0.
|
||||
abs_last_v = abs(last_v)
|
||||
|
||||
# Calcular desplazamiento
|
||||
v2 = v * v
|
||||
last_v2 = last_v * last_v
|
||||
half_inv_accel = .5 / accel if accel != 0 else 0
|
||||
d = (v2 - last_v2) * half_inv_accel
|
||||
_, _, dZ = self.axis.get_point(d)
|
||||
nZ = Z + dZ
|
||||
|
||||
# Ejecutar movimiento
|
||||
toolhead.limit_next_junction_speed(abs_last_v)
|
||||
if v * last_v < 0:
|
||||
# El movimiento primero se detiene y luego cambia de dirección
|
||||
d_decel = -last_v2 * half_inv_accel
|
||||
_, _, decel_Z = self.axis.get_point(d_decel)
|
||||
toolhead.move([X, Y, Z + decel_Z, E], abs_last_v)
|
||||
toolhead.move([X, Y, nZ, E], abs_v)
|
||||
else:
|
||||
toolhead.move([X, Y, nZ, E], max(abs_v, abs_last_v))
|
||||
|
||||
# Actualizar estado y mostrar progreso
|
||||
if math.floor(freq) > math.floor(last_freq):
|
||||
progress = int((current_step / total_steps) * 100)
|
||||
gcmd.respond_info("Probando frecuencia %.1f Hz (Progreso: %d%%)" % (freq, progress))
|
||||
|
||||
Z = nZ
|
||||
last_t = next_t
|
||||
last_v = v
|
||||
last_freq = freq
|
||||
|
||||
# Desacelerar al final si es necesario
|
||||
if last_v:
|
||||
d_decel = -.5 * last_v2 / old_max_accel
|
||||
_, _, decel_Z = self.axis.get_point(d_decel)
|
||||
toolhead.cmd_M204(self.gcode.create_gcode_command(
|
||||
"M204", "M204", {"S": old_max_accel}))
|
||||
toolhead.move([X, Y, Z + decel_Z, E], abs(last_v))
|
||||
|
||||
# Mensaje de finalización
|
||||
gcmd.respond_info("¡Prueba de resonancia completada con éxito!")
|
||||
|
||||
finally:
|
||||
self.is_testing = False
|
||||
# Restaurar configuración original
|
||||
self.gcode.run_script_from_command(
|
||||
"SET_VELOCITY_LIMIT ACCEL=%.3f VELOCITY=%.3f SQUARE_CORNER_VELOCITY=5"
|
||||
% (old_max_accel, old_max_velocity))
|
||||
|
||||
# Restaurar stealthchop si estaba activo
|
||||
if tmc_z is not None and old_thresh is not None:
|
||||
try:
|
||||
tmc_z.set_register("TPWMTHRS", old_thresh)
|
||||
except:
|
||||
gcmd.respond_info("No se pudo restaurar TPWMTHRS")
|
||||
|
||||
# Volver a home en Z solo si no se especificó NO_HOME=1
|
||||
if not no_home:
|
||||
self.gcode.run_script_from_command("G28 Z")
|
||||
gcmd.respond_info("Eje Z homeado después de la prueba")
|
||||
else:
|
||||
gcmd.respond_info("Se omitió el homing del eje Z según lo solicitado")
|
||||
|
||||
# Mensaje final incluso si hubo algún error
|
||||
if not gcmd.get_int("SILENT", 0):
|
||||
gcmd.respond_info("Prueba de resonancia finalizada y configuración restaurada")
|
||||
|
||||
cmd_RESONANCE_TEST_STOP_help = "Detiene la prueba de resonancia en curso"
|
||||
def cmd_RESONANCE_TEST_STOP(self, gcmd):
|
||||
if not self.is_testing:
|
||||
gcmd.respond_info("No hay ninguna prueba de resonancia en curso")
|
||||
return
|
||||
self.is_testing = False
|
||||
gcmd.respond_info("Prueba de resonancia detenida")
|
||||
|
||||
def _setup_test_conditions(self, gcmd, max_accel):
|
||||
# Obtener objetos necesarios
|
||||
reactor = self.printer.get_reactor()
|
||||
toolhead = self.printer.lookup_object('toolhead')
|
||||
|
||||
# Guardar configuración actual
|
||||
systime = reactor.monotonic()
|
||||
toolhead_info = toolhead.get_status(systime)
|
||||
old_max_accel = toolhead_info['max_accel']
|
||||
old_max_velocity = toolhead_info['max_velocity']
|
||||
|
||||
# Variables para restauración
|
||||
tmc_z = None
|
||||
old_thresh = None
|
||||
|
||||
try:
|
||||
# Configurar límites de velocidad más altos para la prueba
|
||||
self.gcode.run_script_from_command(
|
||||
"SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.3f SQUARE_CORNER_VELOCITY=5 VELOCITY=500"
|
||||
% (max_accel, max_accel))
|
||||
|
||||
# Deshabilitar el modo stealthchop durante la prueba
|
||||
tmc_z = self.printer.lookup_object('tmc5160 stepper_z', None)
|
||||
if tmc_z is not None:
|
||||
try:
|
||||
old_thresh = tmc_z.get_register("TPWMTHRS")
|
||||
tmc_z.set_register("TPWMTHRS", 0)
|
||||
except:
|
||||
gcmd.respond_info("No se pudo modificar TPWMTHRS")
|
||||
|
||||
return toolhead, old_max_accel, old_max_velocity, tmc_z, old_thresh
|
||||
|
||||
except Exception as e:
|
||||
if tmc_z is not None and old_thresh is not None:
|
||||
try:
|
||||
tmc_z.set_register("TPWMTHRS", old_thresh)
|
||||
except:
|
||||
pass
|
||||
raise
|
||||
|
||||
def _restore_settings(self, gcmd, old_max_accel, old_max_velocity, tmc_z, old_thresh):
|
||||
# Restaurar configuración original
|
||||
self.gcode.run_script_from_command(
|
||||
"SET_VELOCITY_LIMIT ACCEL=%.3f VELOCITY=%.3f SQUARE_CORNER_VELOCITY=5"
|
||||
% (old_max_accel, old_max_velocity))
|
||||
|
||||
# Restaurar stealthchop si estaba activo
|
||||
if tmc_z is not None and old_thresh is not None:
|
||||
try:
|
||||
tmc_z.set_register("TPWMTHRS", old_thresh)
|
||||
except:
|
||||
gcmd.respond_info("No se pudo restaurar TPWMTHRS")
|
||||
|
||||
# Volver a home en Z solo si no se especificó NO_HOME=1
|
||||
no_home = gcmd.get_int("NO_HOME", 0)
|
||||
skip_pre_home = gcmd.get_int("SKIP_HOME", 0)
|
||||
|
||||
if not no_home and not skip_pre_home:
|
||||
self.gcode.run_script_from_command("G28 Z")
|
||||
gcmd.respond_info("Eje Z homeado después de la prueba")
|
||||
else:
|
||||
gcmd.respond_info("Se omitió el homing del eje Z según lo solicitado")
|
||||
|
||||
cmd_RESONANCE_TEST_FIXED_help = "Ejecuta un movimiento oscilatorio con frecuencia fija"
|
||||
def cmd_RESONANCE_TEST_FIXED(self, gcmd):
|
||||
if self.is_testing:
|
||||
raise gcmd.error("Ya hay una prueba de resonancia en curso")
|
||||
|
||||
# Obtener parámetros para controlar el homing
|
||||
skip_pre_home = gcmd.get_int("SKIP_HOME", 0)
|
||||
|
||||
# Obtener y validar parámetros
|
||||
freq = gcmd.get_float('FREQ', above=0.)
|
||||
if freq > 100000:
|
||||
raise gcmd.error("Frecuencia demasiado alta. Máximo recomendado: 100000 Hz")
|
||||
|
||||
accel_per_hz = gcmd.get_float('ACCEL_PER_HZ', 60., above=0.)
|
||||
duration = gcmd.get_float('DURATION', 5., above=0.)
|
||||
|
||||
# Calcular aceleración basada en ACCEL_PER_HZ
|
||||
accel = accel_per_hz * freq
|
||||
|
||||
# Obtener toolhead
|
||||
toolhead = self.printer.lookup_object('toolhead')
|
||||
|
||||
# Verificar si Z está homeado, si es necesario
|
||||
if not self._check_axis_homed(toolhead, 'z', skip_pre_home):
|
||||
if not skip_pre_home:
|
||||
gcmd.respond_info("Ejecutando G28 Z antes de la prueba...")
|
||||
self.gcode.run_script_from_command("G28 Z")
|
||||
else:
|
||||
gcmd.respond_info("ADVERTENCIA: Eje Z no está homeado pero SKIP_HOME=1 fue especificado")
|
||||
|
||||
try:
|
||||
self.is_testing = True
|
||||
|
||||
# Configurar condiciones de prueba
|
||||
toolhead, old_max_accel, old_max_velocity, tmc_z, old_thresh = self._setup_test_conditions(gcmd, accel)
|
||||
gcmd.respond_info("Configuración inicial completada")
|
||||
|
||||
# Obtener el stepper Z
|
||||
kin = toolhead.get_kinematics()
|
||||
steppers = [s for s in kin.get_steppers() if s.get_name() == 'stepper_z']
|
||||
if not steppers:
|
||||
raise gcmd.error("No se encontró el stepper Z")
|
||||
stepper = steppers[0]
|
||||
|
||||
# Obtener posición actual
|
||||
X, Y, Z, E = toolhead.get_position()
|
||||
gcmd.respond_info("Posición inicial: X=%.3f Y=%.3f Z=%.3f" % (X, Y, Z))
|
||||
|
||||
# Calcular parámetros del movimiento
|
||||
period = 1. / freq # Periodo en segundos
|
||||
t_seg = period / 4. # Tiempo por segmento (1/4 del periodo)
|
||||
mcu = stepper.get_mcu()
|
||||
print_time = mcu.estimated_print_time(toolhead.get_last_move_time())
|
||||
clock = mcu.print_time_to_clock(print_time)
|
||||
|
||||
# Calcular tiempos en ciclos de reloj
|
||||
cycle_ticks = mcu.seconds_to_clock(period)
|
||||
|
||||
# Generar comandos de movimiento
|
||||
gcmd.respond_info("Generando comandos de movimiento...")
|
||||
|
||||
# Obtener el comando de paso
|
||||
step_cmd = mcu.lookup_command(
|
||||
"queue_step oid=%c interval=%u count=%hu add=%hi",
|
||||
"queue_step oid=%c interval=%u count=%hu add=%hi")
|
||||
|
||||
# Calcular parámetros de movimiento
|
||||
interval = cycle_ticks // 4 # Dividir el ciclo en 4 partes
|
||||
steps = 100 # Número de pasos por segmento
|
||||
|
||||
# Enviar comandos de movimiento
|
||||
for i in range(int(duration * freq)):
|
||||
# Primer cuarto (hacia arriba)
|
||||
step_cmd.send([stepper.get_oid(), interval, steps, 1])
|
||||
# Segundo cuarto (desaceleración)
|
||||
step_cmd.send([stepper.get_oid(), interval, steps, -1])
|
||||
# Tercer cuarto (hacia abajo)
|
||||
step_cmd.send([stepper.get_oid(), interval, steps, -1])
|
||||
# Cuarto cuarto (desaceleración)
|
||||
step_cmd.send([stepper.get_oid(), interval, steps, 1])
|
||||
|
||||
if i % 10 == 0:
|
||||
gcmd.respond_info("Ciclo %d enviado" % i)
|
||||
|
||||
gcmd.respond_info("Comandos de movimiento enviados")
|
||||
|
||||
finally:
|
||||
self.is_testing = False
|
||||
self._restore_settings(gcmd, old_max_accel, old_max_velocity, tmc_z, old_thresh)
|
||||
|
||||
def load_config(config):
|
||||
return ResonanceTest(config)
|
||||
831
klippy/extras/zmax_homing.py
Normal file
831
klippy/extras/zmax_homing.py
Normal file
|
|
@ -0,0 +1,831 @@
|
|||
# Módulo para homing del eje Z hacia Zmax usando el endstop de Y
|
||||
#
|
||||
# Copyright (C) 2024 MicroLay
|
||||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
|
||||
import logging
|
||||
|
||||
class ZMaxHomingAlt:
|
||||
def __init__(self, config):
|
||||
self.printer = config.get_printer()
|
||||
self.gcode = self.printer.lookup_object('gcode')
|
||||
|
||||
# Registrar para eventos
|
||||
self.printer.register_event_handler("klippy:connect", self._handle_connect)
|
||||
|
||||
# Variables
|
||||
self.toolhead = None
|
||||
self.y_endstop = None
|
||||
self.z_stepper = None
|
||||
self.endstop_triggered = False
|
||||
self.trigger_position = None
|
||||
|
||||
# Configuración
|
||||
# Velocidades más bajas por defecto
|
||||
self.speed = config.getfloat('speed', 5.0, above=0.)
|
||||
self.second_speed = config.getfloat('second_speed', 2.0, above=0.)
|
||||
self.retract_dist = config.getfloat('retract_dist', 2.0, minval=0.)
|
||||
self.retract_speed = config.getfloat('retract_speed', 3.0, above=0.)
|
||||
# Cambiar a False por defecto para que se quede en Zmax
|
||||
self.final_retract = config.getboolean('final_retract', False)
|
||||
# Evitar que se apaguen los motores al completar
|
||||
self.disable_motors = config.getboolean('disable_motors', False)
|
||||
|
||||
# Registrar comandos
|
||||
self.gcode.register_command(
|
||||
'ZMAX_HOME',
|
||||
self.cmd_ZMAX_HOME,
|
||||
desc=self.cmd_ZMAX_HOME_help
|
||||
)
|
||||
self.gcode.register_command(
|
||||
'MEASURE_Z_LENGTH',
|
||||
self.cmd_MEASURE_Z_LENGTH,
|
||||
desc=self.cmd_MEASURE_Z_LENGTH_help
|
||||
)
|
||||
|
||||
# Mensaje de inicio
|
||||
logging.info(f"ZMaxHoming: Initialized with speeds - homing:{self.speed} mm/s, second:{self.second_speed} mm/s, retract:{self.retract_speed} mm/s")
|
||||
logging.info(f"ZMaxHoming: final_retract={self.final_retract}, disable_motors={self.disable_motors}")
|
||||
|
||||
def _handle_connect(self):
|
||||
# Obtener el toolhead
|
||||
self.toolhead = self.printer.lookup_object('toolhead')
|
||||
|
||||
# Obtener el endstop de Y
|
||||
kin = self.toolhead.get_kinematics()
|
||||
if hasattr(kin, 'rails'):
|
||||
# Buscar el endstop de Y
|
||||
for rail in kin.rails:
|
||||
if rail.get_name() == "stepper_y":
|
||||
endstops = rail.get_endstops()
|
||||
if endstops:
|
||||
self.y_endstop = endstops[0][0]
|
||||
logging.info(f"ZMaxHoming: Using Y endstop for Z-max homing")
|
||||
break
|
||||
|
||||
# Buscar el stepper Z
|
||||
for rail in kin.rails:
|
||||
if rail.get_name() == "stepper_z":
|
||||
self.z_stepper = rail
|
||||
# Obtener la posición máxima de Z
|
||||
z_max_cfg = self.z_stepper.get_range()[1]
|
||||
logging.info(f"ZMaxHoming: Z-max position from config: {z_max_cfg}mm")
|
||||
break
|
||||
|
||||
if self.y_endstop is None:
|
||||
raise self.printer.config_error("No se encontró el endstop de Y")
|
||||
if self.z_stepper is None:
|
||||
raise self.printer.config_error("No se encontró el stepper Z")
|
||||
|
||||
def _monitor_endstop(self):
|
||||
"""Monitorea el endstop Y. Si se activa por primera vez, guarda la posición y actualiza el flag."""
|
||||
# Solo comprobar si aún no hemos detectado el trigger en esta secuencia
|
||||
if not self.endstop_triggered:
|
||||
if self.y_endstop.query_endstop(0):
|
||||
self.endstop_triggered = True
|
||||
# Capturar la posición inmediatamente al detectar el trigger
|
||||
self.trigger_position = self.toolhead.get_position()
|
||||
self.gcode.respond_info(f"!!! Endstop Y TRIGGERED at Z={self.trigger_position[2]:.3f} !!!")
|
||||
# No forzamos la parada aquí, devolvemos True y el llamador decide
|
||||
return True # Triggered NOW
|
||||
# Si ya estaba triggered antes, o si query_endstop dio False, devolvemos el estado actual del flag
|
||||
return self.endstop_triggered
|
||||
|
||||
def _safe_move_z(self, pos, speed, increment):
|
||||
"""Realiza un movimiento seguro en Z con verificación de endstop (pre y post move)"""
|
||||
curpos = self.toolhead.get_position()
|
||||
target_z = pos[2]
|
||||
z_max_cfg = self.z_stepper.get_range()[1]
|
||||
|
||||
# Comprobación inicial antes de cualquier movimiento
|
||||
if self._monitor_endstop():
|
||||
self.gcode.respond_info(f"Endstop triggered before starting safe_move_z at Z={curpos[2]:.3f}")
|
||||
return True
|
||||
|
||||
while curpos[2] < target_z:
|
||||
# Comprobar límites antes de calcular el siguiente paso
|
||||
# Usar una pequeña tolerancia para evitar sobrepasar z_max_cfg
|
||||
if curpos[2] >= z_max_cfg - 0.01:
|
||||
self.gcode.respond_info(f"Approaching Z max limit ({z_max_cfg:.3f}) during safe move. Stopping at Z={curpos[2]:.3f}.")
|
||||
break # Detenerse antes de exceder
|
||||
|
||||
# Calcular siguiente posición, sin sobrepasar target_z ni z_max_cfg
|
||||
next_z = min(curpos[2] + increment, target_z, z_max_cfg - 0.01)
|
||||
# Evitar movimientos extremadamente pequeños si el incremento es muy bajo o ya estamos en el objetivo
|
||||
if abs(next_z - curpos[2]) < 0.001:
|
||||
self.gcode.respond_info(f"Safe move increment too small ({increment:.3f}mm) or target reached at Z={curpos[2]:.3f}. Stopping.")
|
||||
break
|
||||
|
||||
next_pos = list(curpos)
|
||||
next_pos[2] = next_z
|
||||
|
||||
# --- Comprobación Pre-movimiento --- (Dentro del bucle)
|
||||
if self._monitor_endstop():
|
||||
# Endstop activado entre el último movimiento y esta comprobación
|
||||
# La posición ya fue capturada por _monitor_endstop
|
||||
self.gcode.respond_info(f"Endstop check (pre-move): TRIGGERED at Z={self.trigger_position[2]:.3f} (current Z={curpos[2]:.3f})")
|
||||
return True
|
||||
|
||||
try:
|
||||
# Realizar el movimiento
|
||||
# self.gcode.respond_info(f"Moving Z: {curpos[2]:.3f} -> {next_z:.3f} (incr: {increment:.3f}, target: {target_z:.3f})") # Debug
|
||||
self.toolhead.move(next_pos, speed)
|
||||
self.toolhead.wait_moves() # Asegurar que el movimiento se complete ANTES de la comprobación post-movimiento
|
||||
|
||||
# --- Comprobación Post-movimiento --- (CRÍTICO)
|
||||
if self._monitor_endstop():
|
||||
# Endstop activado durante o inmediatamente después del movimiento
|
||||
# La posición ya fue capturada por _monitor_endstop
|
||||
self.gcode.respond_info(f"Endstop check (post-move): TRIGGERED at Z={self.trigger_position[2]:.3f} (intended move Z={next_z:.3f})")
|
||||
return True
|
||||
|
||||
# Actualizar posición para la siguiente iteración si no hubo trigger
|
||||
curpos = next_pos # Actualizar a la posición a la que nos acabamos de mover
|
||||
|
||||
except Exception as e:
|
||||
self.gcode.respond_info(f"Error during toolhead.move in _safe_move_z: {str(e)}")
|
||||
# Comprobar endstop una última vez en caso de error durante el procesamiento del movimiento
|
||||
return self._monitor_endstop()
|
||||
|
||||
# Bucle terminado (alcanzó target_z, z_max_cfg, o incremento muy pequeño)
|
||||
# Comprobación final por si se activó justo cuando la condición del bucle se volvió falsa
|
||||
final_triggered_state = self._monitor_endstop()
|
||||
if not final_triggered_state:
|
||||
self.gcode.respond_info(f"safe_move_z finished loop. TargetZ: {target_z:.3f}, FinalZ: {curpos[2]:.3f}. Endstop triggered flag: {self.endstop_triggered}")
|
||||
|
||||
return self.endstop_triggered # Devolver el estado final del flag
|
||||
|
||||
def _find_z_max(self, gcmd, speed, second_speed, retract_dist, z_homed):
|
||||
"""Encuentra la posición Zmax. Retorna la posición del trigger si z_homed, o [None, None, z_max_cfg] si no."""
|
||||
toolhead = self.toolhead
|
||||
z_max_cfg = self.z_stepper.get_range()[1]
|
||||
z_min_cfg = self.z_stepper.get_range()[0] # Añadido para verificar límites
|
||||
|
||||
self.endstop_triggered = False
|
||||
self.trigger_position = None # Reset trigger position
|
||||
toolhead.flush_step_generation()
|
||||
toolhead.dwell(0.001)
|
||||
|
||||
# Verificar si el endstop ya está activado al inicio
|
||||
initial_endstop_state = self.y_endstop.query_endstop(0)
|
||||
|
||||
if initial_endstop_state:
|
||||
gcmd.respond_info("Endstop Y ya activado al inicio del procedimiento")
|
||||
# Si el endstop ya está activado, registramos la posición actual
|
||||
current_pos = toolhead.get_position()
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = list(current_pos) # Usar list() para crear una copia
|
||||
|
||||
# Si necesitamos retracción y podemos retroceder sin salir del rango válido
|
||||
safe_retract_dist = min(retract_dist, current_pos[2] - z_min_cfg)
|
||||
|
||||
if safe_retract_dist > 0:
|
||||
gcmd.respond_info(f"Retrayendo {safe_retract_dist}mm para desactivar el endstop")
|
||||
retract_pos = list(current_pos)
|
||||
retract_pos[2] -= safe_retract_dist
|
||||
try:
|
||||
toolhead.move(retract_pos, speed)
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Verificar si el endstop se desactivó
|
||||
if not self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info("Endstop desactivado después de la retracción")
|
||||
self.endstop_triggered = False
|
||||
else:
|
||||
gcmd.respond_info("ADVERTENCIA: Endstop sigue activado después de retracción")
|
||||
except Exception as e:
|
||||
gcmd.respond_info(f"Error durante retracción inicial: {str(e)}")
|
||||
else:
|
||||
gcmd.respond_info("No se puede retroceder más sin salir del rango válido")
|
||||
|
||||
# Si el endstop sigue activado después de intentar retracción, no podemos continuar con normalidad
|
||||
if initial_endstop_state and self.endstop_triggered:
|
||||
gcmd.respond_info("El endstop sigue activado, asumiendo que ya estamos en posición Z-max")
|
||||
if not z_homed:
|
||||
# Para Z no homeado, retornamos la posición máxima de configuración con formato completo
|
||||
current_pos = toolhead.get_position()
|
||||
# Asegurar que current_pos tiene 4 elementos
|
||||
if len(current_pos) < 4:
|
||||
current_pos = list(current_pos) + [0.0] * (4 - len(current_pos))
|
||||
return [current_pos[0], current_pos[1], z_max_cfg, current_pos[3]]
|
||||
else:
|
||||
# Para Z homeado, retornamos la posición actual como trigger final
|
||||
return self.trigger_position or toolhead.get_position()
|
||||
|
||||
# Obtener la posición actual
|
||||
current_pos = toolhead.get_position()
|
||||
current_z = current_pos[2]
|
||||
|
||||
# Asegurar que current_pos tiene 4 elementos
|
||||
if len(current_pos) < 4:
|
||||
current_pos = list(current_pos) + [0.0] * (4 - len(current_pos))
|
||||
gcmd.respond_info("Extendiendo posición actual para incluir el valor E")
|
||||
|
||||
# Si estamos cerca de Zmax, proceder directamente a una aproximación directa
|
||||
if z_homed and current_z > 70.0: # Si estamos a más de 70mm (cerca de Zmax que es ~79mm según los logs)
|
||||
gcmd.respond_info(f"Posición actual Z={current_z:.3f} cercana a Zmax, intentando aproximación directa...")
|
||||
# Aproximación directa a Z máximo
|
||||
try:
|
||||
move_pos = list(current_pos)
|
||||
move_pos[2] = z_max_cfg
|
||||
toolhead.move(move_pos, speed/2) # Velocidad reducida para mayor precisión
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Verificar si el endstop se activó
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info("Endstop activado después de aproximación directa a Zmax")
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = toolhead.get_position()
|
||||
|
||||
# Hacemos una retracción pequeña para liberar presión sobre el endstop
|
||||
retract_pos = list(self.trigger_position)
|
||||
retract_pos[2] -= min(1.0, retract_dist) # Retracción más pequeña (1mm o menos)
|
||||
toolhead.move(retract_pos, speed)
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Aproximación final lenta y precisa
|
||||
gcmd.respond_info("Realizando aproximación final de precisión...")
|
||||
move_pos = list(retract_pos)
|
||||
move_pos[2] = z_max_cfg
|
||||
toolhead.move(move_pos, second_speed/2) # Velocidad ultra-reducida
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Verificar estado final
|
||||
final_triggered = self.y_endstop.query_endstop(0)
|
||||
gcmd.respond_info(f"Estado final del endstop: {'ACTIVADO' if final_triggered else 'NO ACTIVADO'}")
|
||||
|
||||
if final_triggered:
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = toolhead.get_position()
|
||||
return self.trigger_position
|
||||
else:
|
||||
# Si por alguna razón no está activado, usar posición máxima de configuración
|
||||
gcmd.respond_info("Endstop no activado en aproximación final, usando posición máxima de configuración")
|
||||
# Usar posición actual pero con Z máximo, asegurando los 4 elementos
|
||||
pos = toolhead.get_position()
|
||||
if len(pos) < 4:
|
||||
pos = list(pos) + [0.0] * (4 - len(pos))
|
||||
self.trigger_position = [pos[0], pos[1], z_max_cfg, pos[3]]
|
||||
return self.trigger_position
|
||||
else:
|
||||
# Si no se activó con la aproximación directa, intentamos una búsqueda normal
|
||||
gcmd.respond_info("Endstop no activado con aproximación directa, intentando búsqueda normal")
|
||||
# No retornamos, continuamos con la búsqueda normal
|
||||
except Exception as e:
|
||||
gcmd.respond_info(f"Error durante aproximación directa: {str(e)}")
|
||||
# Continuamos con búsqueda normal si falla la aproximación directa
|
||||
|
||||
if z_homed:
|
||||
# --- Homing Z cuando ya está homeado (lógica original adaptada) ---
|
||||
gcmd.respond_info("Iniciando búsqueda rápida (absoluta)...")
|
||||
curpos = toolhead.get_position()
|
||||
# Asegurar que curpos tiene 4 elementos
|
||||
if len(curpos) < 4:
|
||||
curpos = list(curpos) + [0.0] * (4 - len(curpos))
|
||||
|
||||
movepos = list(curpos)
|
||||
# Asegurarse de que la posición objetivo no sea igual a la actual si ya estamos cerca del máximo
|
||||
if abs(curpos[2] - z_max_cfg) < 0.1:
|
||||
movepos[2] = curpos[2] - 0.2 # Moverse ligeramente hacia abajo si ya estamos en el límite
|
||||
else:
|
||||
# Si Z ya está homeado, empezar desde una posición más baja para asegurar que
|
||||
# el endstop se active durante el movimiento
|
||||
target_z = max(z_max_cfg - 5.0, curpos[2])
|
||||
# Nunca bajar más de 5mm desde la posición actual
|
||||
target_z = max(target_z, curpos[2] - 5.0)
|
||||
# Nunca subir si ya estamos en una posición alta
|
||||
target_z = min(target_z, z_max_cfg - 0.1)
|
||||
movepos[2] = target_z
|
||||
gcmd.respond_info(f"Posición actual Z: {curpos[2]:.3f}, posición objetivo Z: {target_z:.3f}")
|
||||
|
||||
# Verificar si el endstop ya estaba activado antes de mover
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info("Endstop Y ya activado antes de iniciar movimiento")
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = list(curpos) # Usar la posición actual
|
||||
# No necesitamos hacer el movimiento inicial, continuamos con la retracción
|
||||
else:
|
||||
# Mover y verificar si se activa el endstop
|
||||
if not self._safe_move_z(movepos, speed, 1.0): # Incrementos de 1mm
|
||||
# Si safe_move_z retorna False pero el endstop SÍ se activó, significa que se activó inmediatamente.
|
||||
if not self.endstop_triggered:
|
||||
# Verificar una última vez por si acaso
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info("Endstop se activó en verificación final después de no detectarse en búsqueda")
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = toolhead.get_position()
|
||||
else:
|
||||
# No se encontró el endstop, pero podemos estar en Zmax
|
||||
# Intentar mover un poco más arriba directamente como último recurso
|
||||
gcmd.respond_info("Intentando aproximación directa a Zmax como último recurso...")
|
||||
final_pos = list(toolhead.get_position())
|
||||
# Asegurar que final_pos tiene 4 elementos
|
||||
if len(final_pos) < 4:
|
||||
final_pos = list(final_pos) + [0.0] * (4 - len(final_pos))
|
||||
|
||||
# Intentar subir directamente a Zmax
|
||||
final_pos[2] = z_max_cfg
|
||||
try:
|
||||
toolhead.move(final_pos, speed/2)
|
||||
toolhead.wait_moves()
|
||||
# Verificar si ahora el endstop está activado
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info("Endstop activado después de movimiento directo a Zmax")
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = toolhead.get_position()
|
||||
else:
|
||||
# Intento adicional: moverse un poco más allá de Zmax por si el final de carrera está ligeramente más alto
|
||||
try:
|
||||
gcmd.respond_info("Último intento: movimiento extendido más allá de Zmax...")
|
||||
extended_pos = list(final_pos)
|
||||
extended_pos[2] = z_max_cfg + 0.2 # 0.2mm más allá del máximo configurado
|
||||
toolhead.move(extended_pos, speed/4) # Velocidad muy baja
|
||||
toolhead.wait_moves()
|
||||
# Verificación final
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info("Endstop activado en movimiento extendido")
|
||||
self.endstop_triggered = True
|
||||
self.trigger_position = toolhead.get_position()
|
||||
else:
|
||||
raise gcmd.error("No se detectó el endstop después de intentar todas las opciones")
|
||||
except Exception as e:
|
||||
if "Move out of range" in str(e):
|
||||
gcmd.respond_info("Límite de movimiento alcanzado en intento extendido. Asumiendo Zmax.")
|
||||
self.endstop_triggered = True
|
||||
# Crear trigger_position con 4 elementos, incluyendo E de la posición actual
|
||||
self.trigger_position = [final_pos[0], final_pos[1], z_max_cfg, final_pos[3]]
|
||||
else:
|
||||
raise gcmd.error(f"No se detectó el endstop durante la búsqueda rápida: {str(e)}")
|
||||
except Exception as e:
|
||||
if "Move out of range" in str(e):
|
||||
gcmd.respond_info("Límite de movimiento alcanzado. Asumiendo posición Zmax.")
|
||||
self.endstop_triggered = True
|
||||
# Crear trigger_position con 4 elementos, incluyendo E de la posición actual
|
||||
self.trigger_position = [final_pos[0], final_pos[1], z_max_cfg, final_pos[3]]
|
||||
else:
|
||||
raise gcmd.error(f"No se detectó el endstop durante la búsqueda rápida: {str(e)}")
|
||||
else:
|
||||
gcmd.respond_info("Endstop detectado inmediatamente en búsqueda rápida.")
|
||||
# Si no hay trigger_position guardado aún (pasó en el primer check), tomar la posición actual
|
||||
if not self.trigger_position:
|
||||
self.trigger_position = toolhead.get_position()
|
||||
|
||||
|
||||
# Si _safe_move_z terminó porque se alcanzó el objetivo sin trigger (raro), verificar una vez más
|
||||
if not self.endstop_triggered:
|
||||
if self._monitor_endstop():
|
||||
gcmd.respond_info("Endstop detectado al final del movimiento rápido.")
|
||||
else:
|
||||
# Si realmente no se disparó, es un error
|
||||
raise gcmd.error("Se alcanzó el objetivo de movimiento rápido sin detectar el endstop.")
|
||||
|
||||
|
||||
# Asegurar que tenemos una posición de trigger
|
||||
if not self.trigger_position:
|
||||
# Si por alguna razón no se guardó, tomar la posición actual como referencia (menos preciso)
|
||||
self.trigger_position = toolhead.get_position()
|
||||
gcmd.respond_info("No se guardó la posición exacta del trigger rápido, usando la posición actual.")
|
||||
|
||||
# Asegurar que trigger_position tiene 4 elementos
|
||||
if len(self.trigger_position) < 4:
|
||||
# Obtener el valor E actual
|
||||
current_pos = toolhead.get_position()
|
||||
e_value = 0.0
|
||||
if len(current_pos) >= 4:
|
||||
e_value = current_pos[3]
|
||||
# Extender trigger_position con el valor E
|
||||
self.trigger_position = list(self.trigger_position) + [0.0] * (4 - len(self.trigger_position))
|
||||
self.trigger_position[3] = e_value
|
||||
gcmd.respond_info("Extendiendo posición de trigger para incluir valor E")
|
||||
|
||||
first_trigger_pos = list(self.trigger_position) # Guardar posición donde se activó el endstop
|
||||
|
||||
# Retraer
|
||||
gcmd.respond_info(f"Primer trigger detectado en Z={first_trigger_pos[2]:.3f}. Retrayendo {retract_dist}mm...")
|
||||
retract_pos = list(first_trigger_pos)
|
||||
retract_pos[2] -= retract_dist
|
||||
# Asegurar que la retracción no vaya por debajo de position_min
|
||||
if retract_pos[2] < z_min_cfg:
|
||||
retract_pos[2] = z_min_cfg
|
||||
gcmd.respond_info(f"Ajustando retracción a Z={retract_pos[2]:.3f} para no exceder Zmin")
|
||||
toolhead.move(retract_pos, speed) # Retraer a velocidad normal
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Búsqueda lenta
|
||||
gcmd.respond_info("Iniciando aproximación fina (absoluta)...")
|
||||
self.endstop_triggered = False # Resetear para la segunda búsqueda
|
||||
self.trigger_position = None
|
||||
toolhead.flush_step_generation()
|
||||
toolhead.dwell(0.001)
|
||||
|
||||
# Mover lentamente hacia z_max de nuevo desde la posición retraída
|
||||
movepos = list(retract_pos)
|
||||
# Asegurarse de que la posición objetivo no sea igual a la actual
|
||||
if abs(retract_pos[2] - z_max_cfg) < 0.01:
|
||||
movepos[2] = retract_pos[2] - 0.02 # Moverse ligeramente hacia abajo si ya estamos en el límite
|
||||
else:
|
||||
movepos[2] = z_max_cfg - 0.1
|
||||
|
||||
if not self._safe_move_z(movepos, second_speed, 0.02): # Incrementos de 0.02mm
|
||||
# Si safe_move_z retorna False pero el endstop SÍ se activó
|
||||
if not self.endstop_triggered:
|
||||
# Si falla durante la aproximación lenta (raro, a menos que haya problema con el endstop), usar el primer trigger
|
||||
gcmd.respond_info("No se detectó el endstop en la aproximación fina. Usando posición del primer trigger.")
|
||||
self.trigger_position = first_trigger_pos
|
||||
else:
|
||||
gcmd.respond_info("Endstop detectado inmediatamente en aproximación fina.")
|
||||
if not self.trigger_position:
|
||||
self.trigger_position = toolhead.get_position() # Posición actual como fallback
|
||||
|
||||
# Verificar de nuevo si se alcanzó el objetivo sin trigger
|
||||
if not self.endstop_triggered:
|
||||
if self._monitor_endstop():
|
||||
gcmd.respond_info("Endstop detectado al final de la aproximación fina.")
|
||||
else:
|
||||
# Si no se activó, usar la posición del primer trigger como fallback más seguro
|
||||
gcmd.respond_info("Se alcanzó el objetivo de aproximación fina sin detectar el endstop. Usando posición del primer trigger.")
|
||||
self.trigger_position = first_trigger_pos
|
||||
|
||||
|
||||
if not self.trigger_position:
|
||||
# Fallback final: usar el primer trigger si todo lo demás falla
|
||||
gcmd.respond_info("No se registró la posición del trigger en la aproximación fina. Usando el primer trigger.")
|
||||
self.trigger_position = first_trigger_pos
|
||||
|
||||
# Verificar que trigger_position tiene 4 elementos antes de retornar
|
||||
if len(self.trigger_position) < 4:
|
||||
# Obtener valor E actual
|
||||
current_pos = toolhead.get_position()
|
||||
e_value = 0.0
|
||||
if len(current_pos) >= 4:
|
||||
e_value = current_pos[3]
|
||||
# Extender trigger_position
|
||||
self.trigger_position = list(self.trigger_position) + [0.0] * (4 - len(self.trigger_position))
|
||||
self.trigger_position[3] = e_value
|
||||
gcmd.respond_info("Extendiendo posición de trigger final para incluir valor E")
|
||||
|
||||
gcmd.respond_info(f"Trigger final detectado en Z={self.trigger_position[2]:.3f}")
|
||||
return self.trigger_position # Retornar la posición medida
|
||||
|
||||
else:
|
||||
# --- Homing Z cuando no está homeado (marcar Z como homeado temporalmente) ---
|
||||
gcmd.respond_info("Eje Z no homeado. Marcando Z como homeado temporalmente para permitir movimiento...")
|
||||
# Obtener posición actual (puede ser [0,0,0] o la última conocida antes del reinicio)
|
||||
current_pos = toolhead.get_position()
|
||||
# Asegurar que current_pos tiene 4 elementos
|
||||
if len(current_pos) < 4:
|
||||
current_pos = list(current_pos) + [0.0] * (4 - len(current_pos))
|
||||
|
||||
# Marcar solo Z como homeado en su posición actual
|
||||
toolhead.set_position(current_pos, homing_axes=('z',))
|
||||
toolhead.wait_moves() # Asegurar que el estado se actualiza
|
||||
gcmd.respond_info(f"Z temporalmente homeado en {current_pos[2]:.3f}. Iniciando búsqueda ZMAX...")
|
||||
|
||||
# Si el endstop ya estaba activado y no pudimos desactivarlo, simplemente retornamos
|
||||
if initial_endstop_state and self.endstop_triggered:
|
||||
gcmd.respond_info("Ya en posición Z-max, omitiendo búsqueda")
|
||||
# Retornar posición con formato completo (4 elementos)
|
||||
return [current_pos[0], current_pos[1], z_max_cfg, current_pos[3]]
|
||||
|
||||
# Ahora que Z está "homeado", podemos usar la lógica de movimiento absoluto
|
||||
# Realizar búsqueda rápida
|
||||
gcmd.respond_info("Iniciando búsqueda rápida (absoluta simulada)...")
|
||||
movepos = list(current_pos) # Usar la posición actual "falsa" como punto de partida
|
||||
movepos[2] = z_max_cfg - 0.1 # Objetivo justo debajo del máximo configurado
|
||||
|
||||
# Usamos _safe_move_z, que ahora funcionará porque Z está marcado como homeado
|
||||
if not self._safe_move_z(movepos, speed, 1.0):
|
||||
if not self.endstop_triggered:
|
||||
raise gcmd.error("No se detectó el endstop durante la búsqueda rápida (Z no homeado)")
|
||||
else:
|
||||
gcmd.respond_info("Endstop detectado inmediatamente en búsqueda rápida (Z no homeado).")
|
||||
if not self.trigger_position:
|
||||
self.trigger_position = toolhead.get_position()
|
||||
|
||||
if not self.endstop_triggered:
|
||||
if self._monitor_endstop():
|
||||
gcmd.respond_info("Endstop detectado al final del movimiento rápido (Z no homeado).")
|
||||
else:
|
||||
raise gcmd.error("Se alcanzó el objetivo de movimiento rápido sin detectar el endstop (Z no homeado).")
|
||||
|
||||
if not self.trigger_position:
|
||||
self.trigger_position = toolhead.get_position()
|
||||
gcmd.respond_info("No se guardó la posición exacta del trigger rápido (Z no homeado), usando la posición actual.")
|
||||
|
||||
# Asegurar que trigger_position tiene 4 elementos
|
||||
if len(self.trigger_position) < 4:
|
||||
self.trigger_position = list(self.trigger_position) + [0.0] * (4 - len(self.trigger_position))
|
||||
if len(current_pos) >= 4:
|
||||
self.trigger_position[3] = current_pos[3] # Copiar E de la posición actual
|
||||
gcmd.respond_info("Extendiendo posición de trigger para incluir valor E (Z no homeado)")
|
||||
|
||||
first_trigger_pos = list(self.trigger_position)
|
||||
|
||||
# Retraer (ahora se puede usar toolhead.move)
|
||||
gcmd.respond_info(f"Primer trigger (Z no homeado) detectado en Z={first_trigger_pos[2]:.3f}. Retrayendo {retract_dist}mm...")
|
||||
retract_pos = list(first_trigger_pos)
|
||||
retract_pos[2] -= retract_dist
|
||||
# Verificar límites para evitar movimientos fuera de rango
|
||||
if retract_pos[2] < z_min_cfg:
|
||||
retract_pos[2] = z_min_cfg
|
||||
gcmd.respond_info(f"Ajustando retracción a Z={retract_pos[2]:.3f} para no exceder Zmin")
|
||||
toolhead.move(retract_pos, speed)
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Búsqueda lenta
|
||||
gcmd.respond_info("Iniciando aproximación fina (absoluta simulada)...")
|
||||
self.endstop_triggered = False # Resetear para la segunda búsqueda
|
||||
self.trigger_position = None
|
||||
toolhead.flush_step_generation()
|
||||
toolhead.dwell(0.001)
|
||||
|
||||
movepos = list(retract_pos)
|
||||
movepos[2] = z_max_cfg - 0.1 # Objetivo de nuevo justo debajo del máximo
|
||||
|
||||
if not self._safe_move_z(movepos, second_speed, 0.02):
|
||||
if not self.endstop_triggered:
|
||||
gcmd.respond_info("No se detectó el endstop en la aproximación fina (Z no homeado). Usando posición del primer trigger.")
|
||||
self.trigger_position = first_trigger_pos
|
||||
else:
|
||||
gcmd.respond_info("Endstop detectado inmediatamente en aproximación fina (Z no homeado).")
|
||||
if not self.trigger_position:
|
||||
self.trigger_position = toolhead.get_position()
|
||||
|
||||
if not self.endstop_triggered:
|
||||
if self._monitor_endstop():
|
||||
gcmd.respond_info("Endstop detectado al final de la aproximación fina (Z no homeado).")
|
||||
else:
|
||||
gcmd.respond_info("Se alcanzó el objetivo de aproximación fina sin detectar el endstop (Z no homeado). Usando posición del primer trigger.")
|
||||
self.trigger_position = first_trigger_pos
|
||||
|
||||
if not self.trigger_position:
|
||||
gcmd.respond_info("No se registró la posición del trigger en la aproximación fina (Z no homeado). Usando el primer trigger.")
|
||||
self.trigger_position = first_trigger_pos
|
||||
|
||||
# Asegurar que trigger_position tiene 4 elementos
|
||||
if len(self.trigger_position) < 4:
|
||||
self.trigger_position = list(self.trigger_position) + [0.0] * (4 - len(self.trigger_position))
|
||||
if len(current_pos) >= 4:
|
||||
self.trigger_position[3] = current_pos[3] # Copiar E de la posición actual
|
||||
gcmd.respond_info("Extendiendo posición de trigger final para incluir valor E (Z no homeado)")
|
||||
|
||||
gcmd.respond_info(f"Trigger final (Z no homeado) detectado en Z={self.trigger_position[2]:.3f}")
|
||||
# Retornar posición con formato completo (4 elementos)
|
||||
return [current_pos[0], current_pos[1], z_max_cfg, current_pos[3]]
|
||||
|
||||
cmd_ZMAX_HOME_help = "Realiza el homing del eje Z hacia Zmax usando el endstop de Y"
|
||||
def cmd_ZMAX_HOME(self, gcmd):
|
||||
toolhead = self.toolhead
|
||||
if self.y_endstop is None:
|
||||
raise gcmd.error("ZMAX_HOME: Endstop de Y no inicializado")
|
||||
|
||||
# Obtener parámetros
|
||||
speed = gcmd.get_float('SPEED', self.speed, above=0.)
|
||||
second_speed = gcmd.get_float('SECOND_SPEED', self.second_speed, above=0.)
|
||||
retract_dist = gcmd.get_float('RETRACT_DIST', self.retract_dist, minval=0.)
|
||||
retract_speed = gcmd.get_float('RETRACT_SPEED', self.retract_speed, above=0.)
|
||||
|
||||
# Usar la configuración del archivo printer.cfg
|
||||
# Por defecto mantiene el valor en printer.cfg, sólo lo cambia si se especifica en el comando
|
||||
final_retract = gcmd.get_int('FINAL_RETRACT', -1)
|
||||
if final_retract == -1:
|
||||
# Si no se especificó en el comando, usar el valor configurado
|
||||
final_retract = self.final_retract
|
||||
else:
|
||||
# Si se especificó en el comando, convertir de entero a booleano
|
||||
final_retract = final_retract != 0
|
||||
|
||||
# Mostrar estado para debug
|
||||
gcmd.respond_info(f"Configuración: final_retract={final_retract} (valor en archivo={self.final_retract})")
|
||||
|
||||
# Verificar estado de homing
|
||||
status = toolhead.get_status(self.printer.get_reactor().monotonic())
|
||||
z_homed = 'z' in status['homed_axes']
|
||||
x_homed = 'x' in status['homed_axes']
|
||||
y_homed = 'y' in status['homed_axes']
|
||||
|
||||
gcmd.respond_info(f"ZMAX_HOME: Iniciando con velocidad:{speed} mm/s, velocidad fina:{second_speed} mm/s")
|
||||
if not z_homed:
|
||||
gcmd.respond_info("Eje Z no homeado. Realizando ZMAX_HOME relativo.")
|
||||
else:
|
||||
gcmd.respond_info("ZMAX_HOME: Iniciando búsqueda (eje Z ya homeado)...")
|
||||
|
||||
# Guardar estado Gcode (posición relativa/absoluta)
|
||||
gcode_move = self.printer.lookup_object('gcode_move')
|
||||
gcode_state = gcode_move.get_status()['absolute_coordinates']
|
||||
|
||||
# Verificar estado inicial del endstop
|
||||
initial_endstop_state = self.y_endstop.query_endstop(0)
|
||||
gcmd.respond_info(f"Estado inicial del endstop Y: {'ACTIVADO' if initial_endstop_state else 'NO ACTIVADO'}")
|
||||
|
||||
try:
|
||||
# Encontrar Zmax
|
||||
# Pasamos retract_dist a _find_z_max
|
||||
final_pos_data = self._find_z_max(gcmd, speed, second_speed, retract_dist, z_homed)
|
||||
|
||||
# Verificar que final_pos_data no sea None y tenga el formato esperado
|
||||
if final_pos_data is None:
|
||||
raise gcmd.error("Error: _find_z_max devolvió None")
|
||||
|
||||
# Asegurar que final_pos_data sea una lista con al menos 3 elementos
|
||||
if not isinstance(final_pos_data, (list, tuple)) or len(final_pos_data) < 3:
|
||||
# Si no es una lista válida, usar la posición actual como fallback
|
||||
gcmd.respond_info("Formato de posición inválido, usando posición actual")
|
||||
final_pos_data = toolhead.get_position()
|
||||
|
||||
# Obtener la posición Z máxima de la configuración
|
||||
z_max_cfg = self.z_stepper.get_range()[1]
|
||||
|
||||
# Determinar la posición final Z
|
||||
# Si Z no estaba homeado, la posición final es z_max_cfg.
|
||||
# Si Z estaba homeado, usamos la posición medida del trigger devuelta por _find_z_max.
|
||||
final_z = z_max_cfg if not z_homed or final_pos_data[2] is None else final_pos_data[2]
|
||||
|
||||
# Obtener posición actual completa (X, Y, Z, E)
|
||||
current_pos = toolhead.get_position() # Llama a esto DESPUÉS de _find_z_max que puede cambiar el modo G90/G91
|
||||
|
||||
# Asegurar que current_pos tiene 4 elementos (X, Y, Z, E)
|
||||
if len(current_pos) < 4:
|
||||
gcmd.respond_info(f"Advertencia: la posición actual no tiene 4 elementos: {current_pos}")
|
||||
# Extender la lista a 4 elementos añadiendo 0 para E si es necesario
|
||||
current_pos = list(current_pos) + [0.0] * (4 - len(current_pos))
|
||||
|
||||
# Crear nueva posición manteniendo X, Y y E originales, solo cambiando Z
|
||||
new_pos = list(current_pos) # Copia completa de la posición actual (incluye E)
|
||||
|
||||
# Actualizar X e Y según el estado de homing
|
||||
new_pos[0] = current_pos[0] if x_homed else 0.0 # Usar 0 si no está homeado X
|
||||
new_pos[1] = current_pos[1] if y_homed else 0.0 # Usar 0 si no está homeado Y
|
||||
new_pos[2] = final_z # Establecer Z a la posición final determinada
|
||||
|
||||
if not z_homed:
|
||||
# Si Z no estaba homeado, marcarlo como homeado ahora en z_max_cfg
|
||||
toolhead.set_position(new_pos, homing_axes=('z',))
|
||||
gcmd.respond_info(f"ZMAX_HOME: Eje Z homeado en {final_z:.3f}mm")
|
||||
else:
|
||||
# Si Z ya estaba homeado, simplemente establecer su nueva posición medida
|
||||
toolhead.set_position(new_pos)
|
||||
gcmd.respond_info(f"ZMAX_HOME: Posición Z establecida a {final_z:.3f}mm")
|
||||
|
||||
# Realizar retracción final para liberar presión SOLO si está habilitado
|
||||
if final_retract and retract_dist > 0:
|
||||
try:
|
||||
# Crear una copia nueva de new_pos para la retracción
|
||||
retract_pos = list(new_pos) # Mantener la posición completa (4 elementos)
|
||||
retract_pos[2] = final_z - retract_dist
|
||||
|
||||
# Verificar límites de Z
|
||||
z_min_cfg = self.z_stepper.get_range()[0]
|
||||
if retract_pos[2] < z_min_cfg:
|
||||
retract_pos[2] = z_min_cfg
|
||||
gcmd.respond_info(f"Ajustando retracción final a Z={retract_pos[2]:.3f} para no exceder Zmin")
|
||||
|
||||
gcmd.respond_info(f"ZMAX_HOME: Retracción final a Z={retract_pos[2]:.3f}mm")
|
||||
toolhead.move(retract_pos, retract_speed)
|
||||
toolhead.wait_moves()
|
||||
except Exception as e:
|
||||
# Si falla la retracción final, solo mostramos advertencia pero no abortamos
|
||||
gcmd.respond_info(f"Error en retracción final: {str(e)}")
|
||||
else:
|
||||
# Informar que no se realiza retracción final (quedándose en Zmax)
|
||||
gcmd.respond_info(f"ZMAX_HOME: Sin retracción final, quedando en posición Zmax ({final_z:.3f}mm)")
|
||||
|
||||
# Restaurar estado Gcode
|
||||
if not gcode_state:
|
||||
self.gcode.run_script_from_command("G91")
|
||||
|
||||
# Mensaje final de éxito
|
||||
gcmd.respond_info(f"ZMAX_HOME completado con éxito. Eje Z establecido a {final_z:.3f}mm")
|
||||
|
||||
except Exception as e:
|
||||
# Asegurarse de volver a modo absoluto en caso de error
|
||||
try:
|
||||
self.gcode.run_script_from_command("G90")
|
||||
except Exception:
|
||||
pass # Ignorar errores durante la limpieza
|
||||
# Restaurar estado Gcode anterior si es posible
|
||||
finally:
|
||||
if not gcode_state:
|
||||
try:
|
||||
self.gcode.run_script_from_command("G91")
|
||||
except Exception:
|
||||
pass
|
||||
# Si hay un error, desactivar motores solo si está configurado para hacerlo
|
||||
if self.disable_motors:
|
||||
stepper_enable = self.printer.lookup_object('stepper_enable')
|
||||
stepper_enable.motor_off()
|
||||
gcmd.respond_info("Motores desactivados debido a error.")
|
||||
|
||||
raise gcmd.error(f"Error en ZMAX_HOME: {str(e)}")
|
||||
|
||||
cmd_MEASURE_Z_LENGTH_help = "Mide la longitud real del eje Z entre Zmin y Zmax"
|
||||
def cmd_MEASURE_Z_LENGTH(self, gcmd):
|
||||
try:
|
||||
# Verificar que el sistema está listo
|
||||
if self.y_endstop is None:
|
||||
raise gcmd.error("MEASURE_Z_LENGTH: Endstop de Y no inicializado")
|
||||
|
||||
# Primero hacer home en Zmin usando comandos directos
|
||||
gcmd.respond_info("Iniciando medición - Buscando Zmin...")
|
||||
|
||||
# Obtener el objeto homing
|
||||
toolhead = self.printer.lookup_object('toolhead')
|
||||
homing = self.printer.lookup_object('homing')
|
||||
|
||||
if homing is None:
|
||||
raise gcmd.error("No se pudo acceder al módulo de homing")
|
||||
|
||||
# Realizar homing en Z
|
||||
try:
|
||||
gcmd.respond_info("Ejecutando homing en Z (G28 Z)...")
|
||||
self.gcode.run_script_from_command("G28 Z")
|
||||
gcmd.respond_info("Homing Z completado")
|
||||
except Exception as e:
|
||||
raise gcmd.error(f"Error durante homing Z: {str(e)}")
|
||||
|
||||
# Esperar a que se complete el movimiento
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Obtener la posición después del home
|
||||
z_min_pos = toolhead.get_position()[2]
|
||||
gcmd.respond_info(f"Posición Zmin: {z_min_pos}mm")
|
||||
|
||||
# Mover a una posición intermedia: 5mm por encima de Zmin
|
||||
speed = gcmd.get_float('SPEED', 15, above=0.)
|
||||
self.gcode.run_script_from_command("G91")
|
||||
curpos = list(toolhead.get_position())
|
||||
movepos = list(curpos)
|
||||
movepos[2] = z_min_pos + 5
|
||||
toolhead.move(movepos, speed)
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Fase 1: Aproximación gruesa en tramos de 1mm
|
||||
coarse_travel = 0.0
|
||||
max_coarse = 300 # límite de seguridad en mm
|
||||
gcmd.respond_info("Iniciando aproximación gruesa: incrementos de 1mm...")
|
||||
while coarse_travel < max_coarse:
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info(f"Endstop Y activado en aproximación gruesa tras {coarse_travel:.2f}mm")
|
||||
break
|
||||
self.gcode.run_script_from_command(f"G1 Z1 F{speed*60}")
|
||||
toolhead.wait_moves()
|
||||
coarse_travel += 1.0
|
||||
else:
|
||||
raise gcmd.error("No se activó el endstop Y durante la aproximación gruesa.")
|
||||
|
||||
# Retroceder 2mm para iniciar la aproximación fina
|
||||
gcmd.respond_info("Retrocediendo 2mm para aproximación fina...")
|
||||
self.gcode.run_script_from_command(f"G1 Z-2 F{speed*60}")
|
||||
toolhead.wait_moves()
|
||||
|
||||
# Fase 2: Aproximación fina en tramos de 0.1mm
|
||||
fine_speed = gcmd.get_float('SECOND_SPEED', 2, above=0.)
|
||||
fine_travel = 0.0
|
||||
max_fine = 50 # límite de seguridad en mm
|
||||
gcmd.respond_info("Iniciando aproximación fina: incrementos de 0.1mm...")
|
||||
while fine_travel < max_fine:
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info(f"Endstop Y activado en aproximación fina tras {fine_travel:.2f}mm")
|
||||
break
|
||||
self.gcode.run_script_from_command(f"G1 Z0.1 F{fine_speed*60}")
|
||||
toolhead.wait_moves()
|
||||
fine_travel += 0.1
|
||||
else:
|
||||
raise gcmd.error("No se activó el endstop Y durante la aproximación fina.")
|
||||
|
||||
# Fase 3: Aproximación ultra fina en tramos de 0.01mm
|
||||
gcmd.respond_info("Iniciando fase ultra fina: retrocediendo 0.2mm para iniciar...")
|
||||
self.gcode.run_script_from_command(f"G1 Z-0.2 F{fine_speed*60}")
|
||||
toolhead.wait_moves()
|
||||
|
||||
micro_travel = 0.0
|
||||
max_micro = 10 # límite de seguridad en mm para la fase ultra fina
|
||||
gcmd.respond_info("Iniciando fase ultra fina: incrementos de 0.01mm...")
|
||||
while micro_travel < max_micro:
|
||||
if self.y_endstop.query_endstop(0):
|
||||
gcmd.respond_info(f"Endstop Y activado en fase ultra fina tras {micro_travel:.2f}mm")
|
||||
break
|
||||
self.gcode.run_script_from_command(f"G1 Z0.01 F{fine_speed*60}")
|
||||
toolhead.wait_moves()
|
||||
micro_travel += 0.01
|
||||
else:
|
||||
raise gcmd.error("No se activó el endstop Y durante la fase ultra fina.")
|
||||
|
||||
self.gcode.run_script_from_command("G90")
|
||||
|
||||
# Calcular la longitud total medida desde la posición intermedia
|
||||
# Fórmula: (coarse_travel - 2.2mm) + fine_travel + micro_travel + 1mm
|
||||
z_length = (coarse_travel - 2.2) + fine_travel + micro_travel + 1.0
|
||||
|
||||
gcmd.respond_info(
|
||||
f"Medición completada:\n"
|
||||
f" Posición Zmin: {z_min_pos:.3f}mm\n"
|
||||
f" Longitud total del eje Z: {z_length:.3f}mm"
|
||||
)
|
||||
|
||||
except Exception as e:
|
||||
raise gcmd.error(f"Error en MEASURE_Z_LENGTH: {str(e)}")
|
||||
|
||||
def load_config(config):
|
||||
return ZMaxHomingAlt(config)
|
||||
Loading…
Add table
Add a link
Reference in a new issue