servo: Adding support for startup value for servos (#676)

Signed-off-by: Chris Whiteford <github@chrisandtennille.com>

1 files changed, 25 insertions, 9 deletions

diff --git a/klippy/extras/servo.py b/klippy/extras/servo.py
index e2a188d6..4d3830eb 100644
--- a/klippy/extras/servo.py
+++ b/klippy/extras/servo.py
@@ -28,28 +28,44 @@ class PrinterServo:
         self.gcode.register_mux_command("SET_SERVO", "SERVO", servo_name,
                                         self.cmd_SET_SERVO,
                                         desc=self.cmd_SET_SERVO_help)
-    def set_pwm(self, print_time, value):
+
+        self.initial_pwm_value = None
+        #Check to see if an initial angle or pulse width is configured and
+        # set it as required
+        initial_angle = config.getfloat('initial_angle', None, minval=0., maxval=360.)
+        if initial_angle != None:
+            self.initial_pwm_value = self._get_pwm_from_angle(initial_angle)
+        else:
+            initial_pulse_width = config.getfloat('initial_pulse_width', None, minval=self.min_width, maxval=self.max_width)
+            if initial_pulse_width != None:
+                self.initial_pwm_value = self._get_pwm_from_pulse_width(initial_pulse_width)
+    def printer_state(self, state):
+        if state == 'ready':
+            if self.initial_pwm_value != None:
+                print_time = self.printer.lookup_object('toolhead').get_last_move_time()
+                self._set_pwm(print_time, self.initial_pwm_value)
+    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):
+    def _get_pwm_from_angle(self, 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):
+        return width * self.width_to_value
+    def _get_pwm_from_pulse_width(self, width):
         width = max(self.min_width, min(self.max_width, width))
-        self.set_pwm(print_time, width * self.width_to_value)
+        return width * self.width_to_value
     cmd_SET_SERVO_help = "Set servo angle"
     def cmd_SET_SERVO(self, 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))
+            self._set_pwm(print_time, self._get_pwm_from_pulse_width(
+                self.gcode.get_float('WIDTH', params)))
         else:
-            self.set_angle(print_time, self.gcode.get_float('ANGLE', params))
-
+            self._set_pwm(print_time, self._get_pwm_from_angle(
+                self.gcode.get_float('ANGLE', params)))
 def load_config_prefix(config):
     return PrinterServo(config)