servo: This patch create ability to enable/disable attached servo. (#880)

Cheap mechanical servos have small flickering. When this servo stay on one position, this flickering slowly destroy internal potentiometer and make servo unusable. Many mechanisms need servo only to change position. Therefore I create this minor path to enable/disable servo. It stop pulses for this servo, that's all.

Corresponding G-code is:
SET_SERVO SERVO=config_name [WIDTH=] [ENABLE=<0|1>]
SET_SERVO SERVO=config_name [ANGLE=] [ENABLE=<0|1>]

For example:
SET_SERVO SERVO=touch ANGLE=80 ENABLE=1 ; enable servo and set position
G4 P200 ; wait 200ms
SET_SERVO SERVO=touch ENABLE=0 ; disable servo

This patch add one option to servo configuration:
enable: <False/True> # default True

It not have impact to user code existing already because it is optional parameter and default value is same as original behavior.

Signed-off-by: Jiri Dobry <jdobry@centrum.cz>

3 files changed, 21 insertions, 7 deletions

diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index 29255c48..a493c951 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -478,7 +478,9 @@
 #   the mcu resets.  Must be between minimum_pulse_width and maximum_pulse_width.
 #   This parameter is optional.  If both initial_angle and initial_pulse_width
 #   are set initial_angle will be used
-
+#enable: True
+#   Enable or disable servo. It can be enabled or disabled later using
+#   SET_SERVO SERVO=my_servo ENABLE=<0|1> g-command. The default is True (=enabled)
 
 # Statically configured digital output pins (one may define any number
 # of sections with a "static_digital_output" prefix). Pins configured
diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index 38ae2ca3..fd5291b4 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -142,8 +142,8 @@ is enabled:
 
 The following commands are available when a "servo" config section is
 enabled:
-- `SET_SERVO SERVO=config_name WIDTH=<seconds>`
-- `SET_SERVO SERVO=config_name ANGLE=<degrees>`
+- `SET_SERVO SERVO=config_name [WIDTH=<seconds>] [ENABLE=<0|1>]`
+- `SET_SERVO SERVO=config_name [ANGLE=<degrees>] [ENABLE=<0|1>]`
 
 ## Probe
 
diff --git a/klippy/extras/servo.py b/klippy/extras/servo.py
index 7669d44b..0a33c4d1 100644
--- a/klippy/extras/servo.py
+++ b/klippy/extras/servo.py
@@ -23,6 +23,8 @@ class PrinterServo:
         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.enable = config.getboolean('enable', True)
+        self.last_enable = not self.enable
         servo_name = config.get_name().split()[1]
         self.gcode = self.printer.lookup_object('gcode')
         self.gcode.register_mux_command("SET_SERVO", "SERVO", servo_name,
@@ -49,11 +51,15 @@ class PrinterServo:
                 print_time = 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:
+        if value == self.last_value and self.enable == self.last_enable:
             return
         print_time = max(print_time, self.last_value_time + PIN_MIN_TIME)
-        self.mcu_servo.set_pwm(print_time, value)
+        if self.enable:
+          self.mcu_servo.set_pwm(print_time, value)
+        else:
+          self.mcu_servo.set_pwm(print_time, 0)
         self.last_value = value
+        self.last_enable = self.enable
         self.last_value_time = print_time
     def _get_pwm_from_angle(self, angle):
         angle = max(0., min(self.max_angle, angle))
@@ -65,12 +71,18 @@ class PrinterServo:
     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 'ENABLE' in params:
+            value = self.gcode.get_int('ENABLE', params)
+            self.enable = value != 0
         if 'WIDTH' in params:
             self._set_pwm(print_time, self._get_pwm_from_pulse_width(
                 self.gcode.get_float('WIDTH', params)))
         else:
-            self._set_pwm(print_time, self._get_pwm_from_angle(
-                self.gcode.get_float('ANGLE', params)))
+            if 'ANGLE' in params:
+                self._set_pwm(print_time, self._get_pwm_from_angle(
+                    self.gcode.get_float('ANGLE', params)))
+            else:
+                self._set_pwm(print_time, self.last_value)
 
 def load_config_prefix(config):
     return PrinterServo(config)