diff options
| -rw-r--r-- | klippy/chipmisc.py | 56 | ||||
| -rw-r--r-- | klippy/extras/servo.py | 59 |
2 files changed, 59 insertions, 56 deletions
diff --git a/klippy/chipmisc.py b/klippy/chipmisc.py index 4f5b174f..3874c27a 100644 --- a/klippy/chipmisc.py +++ b/klippy/chipmisc.py @@ -79,60 +79,6 @@ class PrinterPin: ###################################################################### -# Servos -###################################################################### - -SERVO_SIGNAL_PERIOD = 0.020 - -class PrinterServo: - def __init__(self, printer, config): - self.printer = printer - self.mcu_servo = pins.setup_pin(printer, 'pwm', config.get('pin')) - self.mcu_servo.setup_max_duration(0.) - self.mcu_servo.setup_cycle_time(SERVO_SIGNAL_PERIOD) - self.min_width = config.getfloat( - 'minimum_pulse_width', .001, above=0., below=SERVO_SIGNAL_PERIOD) - self.max_width = config.getfloat( - 'maximum_pulse_width', .002 - , above=self.min_width, below=SERVO_SIGNAL_PERIOD) - self.max_angle = config.getfloat('maximum_servo_angle', 180.) - self.angle_to_width = (self.max_width - self.min_width) / self.max_angle - self.width_to_value = 1. / SERVO_SIGNAL_PERIOD - self.last_value = self.last_value_time = 0. - self.gcode = printer.lookup_object('gcode') - self.gcode.register_command("SET_SERVO", self.cmd_SET_SERVO, - desc=self.cmd_SET_SERVO_help) - def set_pwm(self, print_time, value): - if value == self.last_value: - return - print_time = max(print_time, self.last_value_time + PIN_MIN_TIME) - self.mcu_servo.set_pwm(print_time, value) - self.last_value = value - self.last_value_time = print_time - def set_angle(self, print_time, angle): - angle = max(0., min(self.max_angle, angle)) - width = self.min_width + angle * self.angle_to_width - self.set_pwm(print_time, width * self.width_to_value) - def set_pulse_width(self, print_time, width): - width = max(self.min_width, min(self.max_width, width)) - self.set_pwm(print_time, width * self.width_to_value) - cmd_SET_SERVO_help = "Set servo angle" - def cmd_SET_SERVO(self, params): - servo_name = self.gcode.get_str('SERVO', params) - servo = self.printer.lookup_object('servo ' + servo_name, None) - if servo is not self: - if servo is None: - raise self.gcode.error("Servo not configured") - return servo.cmd_SET_SERVO(params) - print_time = self.printer.lookup_object('toolhead').get_last_move_time() - if 'WIDTH' in params: - self.set_pulse_width(print_time, - self.gcode.get_float('WIDTH', params)) - else: - self.set_angle(print_time, self.gcode.get_float('ANGLE', params)) - - -###################################################################### # AD5206 digipot ###################################################################### @@ -386,7 +332,5 @@ def add_printer_objects(printer, config): printer.add_object('pin' + s.section[17:], PrinterPin(printer, s)) for s in config.get_prefix_sections('pwm_output '): printer.add_object('pin' + s.section[10:], PrinterPin(printer, s)) - for s in config.get_prefix_sections('servo '): - printer.add_object(s.section, PrinterServo(printer, s)) for s in config.get_prefix_sections('ad5206 '): printer.add_object(s.section, ad5206(printer, s)) diff --git a/klippy/extras/servo.py b/klippy/extras/servo.py new file mode 100644 index 00000000..547178e8 --- /dev/null +++ b/klippy/extras/servo.py @@ -0,0 +1,59 @@ +# Support for servos +# +# Copyright (C) 2017,2018 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import pins + +SERVO_SIGNAL_PERIOD = 0.020 +PIN_MIN_TIME = 0.100 + +class PrinterServo: + def __init__(self, config): + self.printer = config.get_printer() + self.mcu_servo = pins.setup_pin(self.printer, 'pwm', config.get('pin')) + self.mcu_servo.setup_max_duration(0.) + self.mcu_servo.setup_cycle_time(SERVO_SIGNAL_PERIOD) + self.min_width = config.getfloat( + 'minimum_pulse_width', .001, above=0., below=SERVO_SIGNAL_PERIOD) + self.max_width = config.getfloat( + 'maximum_pulse_width', .002 + , above=self.min_width, below=SERVO_SIGNAL_PERIOD) + self.max_angle = config.getfloat('maximum_servo_angle', 180.) + self.angle_to_width = (self.max_width - self.min_width) / self.max_angle + self.width_to_value = 1. / SERVO_SIGNAL_PERIOD + self.last_value = self.last_value_time = 0. + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command("SET_SERVO", self.cmd_SET_SERVO, + desc=self.cmd_SET_SERVO_help) + def set_pwm(self, print_time, value): + if value == self.last_value: + return + print_time = max(print_time, self.last_value_time + PIN_MIN_TIME) + self.mcu_servo.set_pwm(print_time, value) + self.last_value = value + self.last_value_time = print_time + def set_angle(self, print_time, angle): + angle = max(0., min(self.max_angle, angle)) + width = self.min_width + angle * self.angle_to_width + self.set_pwm(print_time, width * self.width_to_value) + def set_pulse_width(self, print_time, width): + width = max(self.min_width, min(self.max_width, width)) + self.set_pwm(print_time, width * self.width_to_value) + cmd_SET_SERVO_help = "Set servo angle" + def cmd_SET_SERVO(self, params): + servo_name = self.gcode.get_str('SERVO', params) + servo = self.printer.lookup_object('servo ' + servo_name, None) + if servo is not self: + if servo is None: + raise self.gcode.error("Servo not configured") + return servo.cmd_SET_SERVO(params) + print_time = self.printer.lookup_object('toolhead').get_last_move_time() + if 'WIDTH' in params: + self.set_pulse_width(print_time, + self.gcode.get_float('WIDTH', params)) + else: + self.set_angle(print_time, self.gcode.get_float('ANGLE', params)) + +def load_config(config): + return PrinterServo(config) |