shepherd-agent/shepherd/plugins/betterservo.py

from gpiozero import PWMOutputDevice, SourceMixin, CompositeDevice


class BetterServo(SourceMixin, CompositeDevice):
    """
    Copy of GPIOZero servo, but with control over pulse width and active_high
    """
    def __init__(
            self, pin=None, initial_value=0.0,
            min_pulse_width=1/1000, max_pulse_width=2/1000,
            frame_width=20/1000, pin_factory=None, active_high=True):
        if min_pulse_width >= max_pulse_width:
            raise ValueError('min_pulse_width must be less than max_pulse_width')
        if max_pulse_width >= frame_width:
            raise ValueError('max_pulse_width must be less than frame_width')
        self._frame_width = frame_width
        self._min_dc = min_pulse_width / frame_width
        self._dc_range = (max_pulse_width - min_pulse_width) / frame_width
        self._min_value = -1
        self._value_range = 2
        super(BetterServo, self).__init__(
            pwm_device=PWMOutputDevice(
                pin, frequency=int(1 / frame_width), pin_factory=pin_factory,
                active_high=False
            ),
            pin_factory=pin_factory
        )
        self.pwm_device.active_high=active_high
        try:
            self.value = initial_value
        except:
            self.close()
            raise

    @property
    def frame_width(self):
        """
        The time between control pulses, measured in seconds.
        """
        return self._frame_width

    @property
    def min_pulse_width(self):
        """
        The control pulse width corresponding to the servo's minimum position,
        measured in seconds.
        """
        return self._min_dc * self.frame_width

    @property
    def max_pulse_width(self):
        """
        The control pulse width corresponding to the servo's maximum position,
        measured in seconds.
        """
        return (self._dc_range * self.frame_width) + self.min_pulse_width

    @property
    def pulse_width(self):
        """
        Returns the current pulse width controlling the servo.
        """
        if self.pwm_device.frequency is None:
            return None
        else:
            return self.pwm_device.state * self.frame_width

    @pulse_width.setter
    def pulse_width(self, value):
        if value is None:
            self.pwm_device.frequency = None
        elif self.min_pulse_width <= value <= self.max_pulse_width:
            self.pwm_device.frequency = int(1 / self.frame_width)
            self.pwm_device.value = (value / self.frame_width)
        else:
            raise OutputDeviceBadValue("Servo pulse_width must be between min and max supplied during construction, or None")

    def min(self):
        """
        Set the servo to its minimum position.
        """
        self.value = -1

    def mid(self):
        """
        Set the servo to its mid-point position.
        """
        self.value = 0

    def max(self):
        """
        Set the servo to its maximum position.
        """
        self.value = 1

    def detach(self):
        """
        Temporarily disable control of the servo. This is equivalent to
        setting :attr:`value` to ``None``.
        """
        self.value = None

    def _get_value(self):
        if self.pwm_device.frequency is None:
            return None
        else:
            return (
                ((self.pwm_device.state - self._min_dc) / self._dc_range) *
                self._value_range + self._min_value)

    @property
    def value(self):
        """
        Represents the position of the servo as a value between -1 (the minimum
        position) and +1 (the maximum position). This can also be the special
        value ``None`` indicating that the servo is currently "uncontrolled",
        i.e. that no control signal is being sent. Typically this means the
        servo's position remains unchanged, but that it can be moved by hand.
        """
        result = self._get_value()
        if result is None:
            return result
        else:
            # NOTE: This round() only exists to ensure we don't confuse people
            # by returning 2.220446049250313e-16 as the default initial value
            # instead of 0. The reason _get_value and _set_value are split
            # out is for descendents that require the un-rounded values for
            # accuracy
            return round(result, 14)

    @value.setter
    def value(self, value):
        if value is None:
            self.pwm_device.frequency = None
        elif -1 <= value <= 1:
            self.pwm_device.frequency = int(1 / self.frame_width)
            self.pwm_device.value = (
                self._min_dc + self._dc_range *
                ((value - self._min_value) / self._value_range)
                )
        else:
            raise OutputDeviceBadValue(
                "Servo value must be between -1 and 1, or None")

    @property
    def is_active(self):
        return self.value is not None