CreatBot/CreatBotKlipper
CreatBot/CreatBotKlipper
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

分支合并后，整合舵机降速和舵机信号休眠的功能

Browse Source
This commit is contained in:
老五 2025-03-12 08:41:03 +08:00
parent 5e3b7065b0
commit 1c899ac214
1 changed files with 23 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
klippy/extras/servo.py
View File

@ -17,6 +17,10 @@ class PrinterServo:
below=SERVO_SIGNAL_PERIOD) below=SERVO_SIGNAL_PERIOD)
self.max_angle = config.getfloat('maximum_servo_angle', 180.) self.max_angle = config.getfloat('maximum_servo_angle', 180.)
self.signal_duration = config.getfloat('signal_duration', 0, minval=0.) self.signal_duration = config.getfloat('signal_duration', 0, minval=0.)
if self.signal_duration:
self.signal_duration = max(self.signal_duration, SERVO_SIGNAL_PERIOD)
if abs(self.signal_duration / SERVO_SIGNAL_PERIOD - round(self.signal_duration / SERVO_SIGNAL_PERIOD)) < 1e-9:
self.signal_duration -= 0.001
self.steps_decomposed = config.getint('steps_decomposed', 0) self.steps_decomposed = config.getint('steps_decomposed', 0)
self.angle_to_width = (self.max_width - self.min_width) / self.max_angle self.angle_to_width = (self.max_width - self.min_width) / self.max_angle
self.width_to_value = 1. / SERVO_SIGNAL_PERIOD self.width_to_value = 1. / SERVO_SIGNAL_PERIOD
@ -39,7 +43,7 @@ class PrinterServo:
self.printer.register_event_handler("klippy:ready", self._handle_ready) self.printer.register_event_handler("klippy:ready", self._handle_ready)
# Create gcode request queue # Create gcode request queue
self.gcrq = output_pin.GCodeRequestQueue( self.gcrq = output_pin.GCodeRequestQueue(
config, self.mcu_servo.get_mcu(), self._set_pwm) config, self.mcu_servo.get_mcu(), self.__set_pwm)
# Register commands # Register commands
servo_name = config.get_name().split()[1] servo_name = config.get_name().split()[1]
gcode = self.printer.lookup_object('gcode') gcode = self.printer.lookup_object('gcode')
@ -48,39 +52,37 @@ class PrinterServo:
desc=self.cmd_SET_SERVO_help) desc=self.cmd_SET_SERVO_help)
def get_status(self, eventtime): def get_status(self, eventtime):
return {'value': self.last_value} return {'value': self.last_value}
def __set_pwm(self, print_time, value):
if self.steps_decomposed:
return self._set_low_pwm(print_time, value)
else:
return self._set_pwm(print_time, value)
def _set_pwm(self, print_time, value): def _set_pwm(self, print_time, value):
if value == self.last_value: if value == self.last_value:
return "discard", 0. return "discard", 0.
self.last_value = value self.last_value = value
self.mcu_servo.set_pwm(print_time, value) self.mcu_servo.set_pwm(print_time, value)
self._handle_signal_duration(print_time) if self.signal_duration:
self.mcu_servo.set_pwm(print_time + self.signal_duration, 0)
def _get_s_curve_value(self, last_value, value, t): def _get_s_curve_value(self, last_value, value, t):
smooth_factor = t * t * (3 - 2 * t) smooth_factor = t * t * (3 - 2 * t)
return last_value + smooth_factor * (value - last_value) return last_value + smooth_factor * (value - last_value)
def _set_low_pwm(self, print_time, value): def _set_low_pwm(self, print_time, value):
if value == self.last_value: if value == self.last_value:
return return "discard", 0.
if self.last_value == 0: enter_value = self.initial_pwm if self.last_value == 0 else self.last_value
self.last_value = self.initial_pwm self.last_value = value
steps = self.steps_decomposed steps = self.steps_decomposed
for step in range(steps): for step in range(steps):
t = step / (steps - 1) t = step / (steps - 1)
current_value = self._get_s_curve_value(self.last_value, value, t) current_value = self._get_s_curve_value(enter_value, value, t)
print_time = max(print_time, self.last_value_time + 0.02) next_time = print_time + SERVO_SIGNAL_PERIOD * step
self.mcu_servo.set_pwm(print_time, current_value) self.mcu_servo.set_pwm(next_time, current_value)
self.last_value_time = print_time
self.last_value = value
self._handle_signal_duration(print_time)
def _handle_signal_duration(self, print_time):
if self.signal_duration: if self.signal_duration:
if abs(self.signal_duration / 0.02 - round(self.signal_duration / 0.02)) < 1e-9: next_time = print_time + SERVO_SIGNAL_PERIOD * steps + self.signal_duration
self.signal_duration -= 0.001 self.mcu_servo.set_pwm(next_time, 0)
print_time = max(print_time + SERVO_SIGNAL_PERIOD, print_time + self.signal_duration)
self.mcu_servo.set_pwm(print_time, 0)
self.last_value_time = print_time
def _handle_ready(self): def _handle_ready(self):
print_time = self.printer.lookup_object('toolhead').get_last_move_time() self.gcrq.queue_gcode_request(self.initial_pwm)
self._set_pwm(print_time, self.initial_pwm)
def _get_pwm_from_angle(self, angle): def _get_pwm_from_angle(self, angle):
angle = max(0., min(self.max_angle, angle)) angle = max(0., min(self.max_angle, angle))
width = self.min_width + angle * self.angle_to_width width = self.min_width + angle * self.angle_to_width
@ -91,18 +93,11 @@ class PrinterServo:
return width * self.width_to_value return width * self.width_to_value
cmd_SET_SERVO_help = "Set servo angle" cmd_SET_SERVO_help = "Set servo angle"
def cmd_SET_SERVO(self, gcmd): def cmd_SET_SERVO(self, gcmd):
print_time = max(print_time, self.last_value_time)
width = gcmd.get_float('WIDTH', None) width = gcmd.get_float('WIDTH', None)
if width is not None: if width is not None:
if self.steps_decomposed:
self._set_low_pwm(print_time, self._get_pwm_from_pulse_width(width))
else:
value = self._get_pwm_from_pulse_width(width) value = self._get_pwm_from_pulse_width(width)
else: else:
angle = gcmd.get_float('ANGLE') angle = gcmd.get_float('ANGLE')
if self.steps_decomposed:
self._set_low_pwm(print_time, self._get_pwm_from_angle(angle))
else:
value = self._get_pwm_from_angle(angle) value = self._get_pwm_from_angle(angle)
self.gcrq.queue_gcode_request(value) self.gcrq.queue_gcode_request(value)