chipmisc: Add initial support for servos

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 43 insertions, 0 deletions

diff --git a/klippy/chipmisc.py b/klippy/chipmisc.py
index de4a5b9c..454754ff 100644
--- a/klippy/chipmisc.py
+++ b/klippy/chipmisc.py
@@ -33,6 +33,47 @@ class PrinterStaticPWM:
 
 
 ######################################################################
+# Servos
+######################################################################
+
+SERVO_MIN_TIME = 0.100
+SERVO_SIGNAL_PERIOD = 0.020
+
+class PrinterServo:
+    def __init__(self, printer, config):
+        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.
+    def set_pwm(self, print_time, value):
+        if value == self.last_value:
+            return
+        print_time = max(self.last_value_time + SERVO_MIN_TIME, print_time)
+        self.mcu_servo.set_pwm(print_time, value)
+        self.last_value = value
+        self.last_value_time = print_time
+    # External commands
+    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)
+
+def get_printer_servo(printer, name):
+    return printer.objects.get('servo ' + name)
+
+
+######################################################################
 # AD5206 digipot
 ######################################################################
 
@@ -238,5 +279,7 @@ def add_printer_objects(printer, config):
         printer.add_object(s.section, PrinterStaticDigitalOut(printer, s))
     for s in config.get_prefix_sections('static_pwm_output '):
         printer.add_object(s.section, PrinterStaticPWM(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))