chipmisc: Add initial support for servos
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
parent
3b4088c23f
commit
6dcc44ce2d
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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...")
|
||||
|
|
|
|||
Loading…
Reference in New Issue