diff options
| -rw-r--r-- | config/example-extras.cfg | 9 | ||||
| -rw-r--r-- | klippy/extras/servo.py | 34 |
2 files changed, 34 insertions, 9 deletions
diff --git a/config/example-extras.cfg b/config/example-extras.cfg index a2bb5764..e3424eec 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -465,6 +465,15 @@ # The maximum pulse width time (in seconds). This should correspond # with an angle of maximum_servo_angle. The default is 0.002 # seconds. +#initial_angle: 70 +# Initial angle to set the servo to when the mcu resets. Must be between +# 0 and maximum_servo_angle This parameter is optional. If both +# initial_angle and initial_pulse_width are set initial_angle will be used. +#initial_pulse_width: 0.0015 +# Initial pulse width time (in seconds) to set the servo to when +# 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 # Statically configured digital output pins (one may define any number 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) |