diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index 7b943218..0e712f7e 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -53,6 +53,25 @@
 # See the "mcu" section in example.cfg for configuration parameters.
 
 
+# Servos (one may define any number of sections with a "servo"
+# prefix). The servos may be controlled using the SET_SERVO g-code
+# command. For example: SET_SERVO SERVO=my_servo ANGLE=180
+#[servo my_servo]
+#pin: ar7
+#   PWM output pin controlling the servo. This parameter must be
+#   provided.
+#maximum_servo_angle: 180
+#   The maximum angle (in degrees) that this servo can be set to. The
+#   default is 180 degrees.
+#minimum_pulse_width: 0.001
+#   The minimum pulse width time (in seconds). This should correspond
+#   with an angle of 0 degrees. The default is 0.001 seconds.
+#maximum_pulse_width: 0.002
+#   The maximum pulse width time (in seconds). This should correspond
+#   with an angle of maximum_servo_angle. The default is 0.002
+#   seconds.
+
+
 # Statically configured digital output pins (one may define any number
 # of sections with a "static_digital_output" prefix). Pins configured
 # here will be setup as a GPIO output during MCU configuration.
diff --git a/klippy/chipmisc.py b/klippy/chipmisc.py
index de4a5b9c..454754ff 100644
--- a/klippy/chipmisc.py
+++ b/klippy/chipmisc.py
@@ -32,6 +32,47 @@ class PrinterStaticPWM:
         mcu_pwm.setup_static_pwm(value / scale)
 
 
+######################################################################
+# 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))
diff --git a/klippy/gcode.py b/klippy/gcode.py
index c66b74c5..ccf78fac 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -4,7 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import os, re, logging, collections
-import homing, extruder
+import homing, extruder, chipmisc
 
 # Parse out incoming GCode and find and translate head movements
 class GCodeParser:
@@ -81,7 +81,7 @@ class GCodeParser:
         for eventtime, data in self.input_log:
             logging.info("Read %f: %s" % (eventtime, repr(data)))
     # Parse input into commands
-    args_r = re.compile('([a-zA-Z_]+|[a-zA-Z*])')
+    args_r = re.compile('([A-Z_]+|[A-Z*])')
     def process_commands(self, commands, need_ack=True):
         prev_need_ack = self.need_ack
         for line in commands:
@@ -91,17 +91,17 @@ class GCodeParser:
             if cpos >= 0:
                 line = line[:cpos]
             # Break command into parts
-            parts = self.args_r.split(line)[1:]
-            params = { parts[i].upper(): parts[i+1].strip()
+            parts = self.args_r.split(line.upper())[1:]
+            params = { parts[i]: parts[i+1].strip()
                        for i in range(0, len(parts), 2) }
             params['#original'] = origline
-            if parts and parts[0].upper() == 'N':
+            if parts and parts[0] == 'N':
                 # Skip line number at start of command
                 del parts[:2]
             if not parts:
                 self.cmd_default(params)
                 continue
-            params['#command'] = cmd = parts[0].upper() + parts[1].strip()
+            params['#command'] = cmd = parts[0] + parts[1].strip()
             # Invoke handler for command
             self.need_ack = need_ack
             handler = self.gcode_handlers.get(cmd, self.cmd_default)
@@ -187,6 +187,22 @@ class GCodeParser:
         if default is not None:
             return default
         raise error("Error on '%s': missing %s" % (params['#original'], name))
+    extended_r = re.compile(
+        r'^\s*(?:N[0-9]+\s*)?'
+        r'(?P<cmd>[a-zA-Z_][a-zA-Z_]+)(?:\s+|$)'
+        r'(?P<args>[^#*;]*?)'
+        r'\s*(?:[#*;].*)?$')
+    def get_extended_params(self, params):
+        m = self.extended_r.match(params['#original'])
+        if m is None:
+            # Not an "extended" command
+            return params
+        eargs = m.group('args')
+        try:
+            eparams = [earg.split('=', 1) for earg in eargs.split()]
+            return { k.upper(): v for k, v in eparams }
+        except ValueError as e:
+            raise error("Malformed command '%s'" % (params['#original'],))
     # Temperature wrappers
     def get_temp(self):
         if not self.is_printer_ready:
@@ -278,8 +294,8 @@ class GCodeParser:
         'G1', 'G4', 'G20', 'G28', 'G90', 'G91', 'G92',
         'M82', 'M83', 'M18', 'M105', 'M104', 'M109', 'M112', 'M114', 'M115',
         'M140', 'M190', 'M106', 'M107', 'M206', 'M400',
-        'IGNORE', 'QUERY_ENDSTOPS', 'PID_TUNE', 'RESTART', 'FIRMWARE_RESTART',
-        'ECHO', 'STATUS', 'HELP']
+        'IGNORE', 'QUERY_ENDSTOPS', 'PID_TUNE', 'SET_SERVO',
+        'RESTART', 'FIRMWARE_RESTART', 'ECHO', 'STATUS', 'HELP']
     cmd_G1_aliases = ['G0']
     def cmd_G1(self, params):
         # Move
@@ -444,6 +460,20 @@ class GCodeParser:
         temp = self.get_float('S', params)
         heater.start_auto_tune(temp)
         self.bg_temp(heater)
+    cmd_SET_SERVO_help = "Set servo angle"
+    def cmd_SET_SERVO(self, params):
+        params = self.get_extended_params(params)
+        name = params.get('SERVO')
+        if name is None:
+            raise error("Error on '%s': missing SERVO" % (params['#original'],))
+        s = chipmisc.get_printer_servo(self.printer, name)
+        if s is None:
+            raise error("Servo not configured")
+        print_time = self.toolhead.get_last_move_time()
+        if 'WIDTH' in params:
+            s.set_pulse_width(print_time, self.get_float('WIDTH', params))
+            return
+        s.set_angle(print_time, self.get_float('ANGLE', params))
     def prep_restart(self):
         if self.is_printer_ready:
             self.respond_info("Preparing to restart...")