From c5d94a74a5fa8969245f8b03b400ab9b31ec67ff Mon Sep 17 00:00:00 2001 From: Chris Whiteford Date: Fri, 5 Oct 2018 14:35:38 -0400 Subject: servo: Adding support for startup value for servos (#676) Signed-off-by: Chris Whiteford --- klippy/extras/servo.py | 34 +++++++++++++++++++++++++--------- 1 file changed, 25 insertions(+), 9 deletions(-) (limited to 'klippy/extras/servo.py') 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) -- cgit v1.2.3-70-g09d2