diff options
| -rw-r--r-- | config/example-extras.cfg | 19 | ||||
| -rw-r--r-- | klippy/chipmisc.py | 43 | ||||
| -rw-r--r-- | klippy/gcode.py | 46 |
3 files changed, 100 insertions, 8 deletions
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 @@ -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)) 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...") |