hexapod/software/raspberry pi/adafruit_servokit.py

# SPDX-FileCopyrightText: 2018 Kattni Rembor for Adafruit Industries
#
# SPDX-License-Identifier: MIT
#
# MODIFIED to use pololu motor drivers, but compatible with adafruit_servokit driver layout
# by Amelia Deck
# Based on adafruit_motor.py as well

"""
`adafruit_servokit`
====================================================

CircuitPython helper library for the PWM/Servo FeatherWing, Shield and Pi HAT and Bonnet kits.

* Author(s): Kattni Rembor

Implementation Notes
--------------------

**Hardware:**

* `8-Channel PWM or Servo FeatherWing <https://www.adafruit.com/product/2928>`_
* `Adafruit 16-Channel 12-bit PWM/Servo Shield <https://www.adafruit.com/product/1411>`_
* `Adafruit 16-Channel PWM/Servo HAT for Raspberry Pi <https://www.adafruit.com/product/2327>`_
* `Adafruit 16-Channel PWM/Servo Bonnet for Raspberry Pi <https://www.adafruit.com/product/3416>`_

**Software and Dependencies:**

* Adafruit CircuitPython firmware for the supported boards:
  https://github.com/adafruit/circuitpython/releases

* Adafruit's Bus Device library: https://github.com/adafruit/Adafruit_CircuitPython_BusDevice
* Adafruit's Register library: https://github.com/adafruit/Adafruit_CircuitPython_Register
* Adafruit's PCA9685 library: https://github.com/adafruit/Adafruit_CircuitPython_PCA9685
* Adafruit's Motor library: https://github.com/adafruit/Adafruit_CircuitPython_Motor

"""

#import board
#from adafruit_pca9685 import PCA9685

try:
    from typing import Optional
    import maestro
    import time
    #from busio import I2C
    #from adafruit_motor.servo import Servo, ContinuousServo
except ImportError:
    pass

__version__ = "0.0.0+auto.0"
__repo__ = "https://github.com/adafruit/Adafruit_CircuitPython_ServoKit.git"

pca = None

class ServoKit:
    """Class representing an Adafruit PWM/Servo FeatherWing, Shield or Pi HAT and Bonnet kits.

    Automatically uses the I2C bus on a Feather, Metro or Raspberry Pi.

    Initialise the PCA9685 chip at ``address``.

    The internal reference clock is 25MHz but may vary slightly with environmental conditions and
    manufacturing variances. Providing a more precise ``reference_clock_speed`` can improve the
    accuracy of the frequency and duty_cycle computations. See the ``calibration.py`` example in
    the `PCA9685 GitHub repo <https://github.com/adafruit/Adafruit_CircuitPython_PCA9685>`_ for
    how to derive this value by measuring the resulting pulse widths.

    :param int channels: The number of servo channels available. Must be 8 or 16. The FeatherWing
                         has 8 channels. The Shield, HAT, and Bonnet have 16 channels.
    :param ~I2C i2c: The I2C bus to use. If not provided, it will use generate the default I2C
                                      bus singleton ``busio.I2C()`` and use that.
    :param int address: The I2C address of the PCA9685. Default address is ``0x40``.
    :param int reference_clock_speed: The frequency of the internal reference clock in Hertz.
                                      Default reference clock speed is ``25000000``.
    :param int frequency: The overall PWM frequency of the PCA9685 in Hertz.
                                      Default frequency is ``50``.

    """

    def __init__(
        self,
        *,
        channels: int,
        #i2c: Optional[I2C] = None,
        address: str = "/dev/ttyACM0",
        reference_clock_speed: int = 25000000,
        frequency: int = 50 # actually, device ID over serial pololu protocol. using existing var names for full compatibility!
    ) -> None:
        if channels not in [6, 12, 18, 24]:
            raise ValueError("servo_channels must be 6, 12, 18, 24!")
        self._items = [None] * channels
        self._channels = channels
        #if i2c is None:
        #    i2c = board.I2C()
        #self._pca = PCA9685(
        #    i2c, address=address, reference_clock_speed=reference_clock_speed
        #)
        #self._pca.frequency = frequency
        global pca
        self._pca = maestro.Controller(ttyStr=address,device=frequency)
        pca = self._pca
        self._servo = _Servo(self)

    @property
    def servo(self) -> "_Servo":
        """:class:`~adafruit_motor.servo.Servo` controls for standard servos.

        This FeatherWing example rotates a servo on channel ``0`` to ``180`` degrees for one second,
        and then returns it to ``0`` degrees.

        .. code-block:: python

            import time
            from adafruit_servokit import ServoKit

            kit = ServoKit(channels=8)

            kit.servo[0].angle = 180
            time.sleep(1)
            kit.servo[0].angle = 0

        """
        return self._servo


class _Servo:
    # pylint: disable=protected-access
    def __init__(self, kit: ServoKit) -> None:
        self.kit = kit

    def __getitem__(self, servo_channel: int):
        #import adafruit_motor.servo  # pylint: disable=import-outside-toplevel

        num_channels = self.kit._channels
        if servo_channel >= num_channels or servo_channel < 0:
            raise ValueError("servo must be 0-{}!".format(num_channels - 1))
        servo = pololuservo(servo_channel)
        
        return servo
        raise ValueError("Channel {} is already in use.".format(servo_channel))

    def __len__(self) -> int:
        return len(self.kit._items)

class pololuservo:
    def __init__(self, ch, actuation_range: int = 180, min_pulse: int = 1600, max_pulse: int = 9900):
        self.ch = ch
        self.actuation_range = actuation_range
        self._min_duty = min_pulse
        self._max_duty = max_pulse
        self._duty_range = max_pulse - min_pulse
        self.duty_cycle = 0
        print("init servo", ch)
        pca.setAccel(ch, 15)
        pca.setTarget(ch, int((max_pulse-min_pulse)/2 + min_pulse)) # set midpoint
        time.sleep(0.1)
        pca.setAccel(ch, 0)


    @property
    def angle(self) -> Optional[float]:
        """The servo angle in degrees. Must be in the range ``0`` to ``actuation_range``.
        Is None when servo is disabled."""
        if self.fraction is None:  # special case for disabled servos
            return None
        return self.actuation_range * self.fraction
    
    @angle.setter
    def angle(self, new_angle: Optional[float]) -> None:
        if new_angle is None:  # disable the servo by sending 0 signal
            self.fraction = None
            return
        if new_angle < 0 or new_angle > self.actuation_range:
            raise ValueError("Angle out of range")
        self.fraction = new_angle / self.actuation_range

    @property
    def fraction(self) -> Optional[float]:
        """Pulse width expressed as fraction between 0.0 (`min_pulse`) and 1.0 (`max_pulse`).
        For conventional servos, corresponds to the servo position as a fraction
        of the actuation range. Is None when servo is diabled (pulsewidth of 0ms).
        """
        if self.duty_cycle == 0:  # Special case for disabled servos
            return None
        return (self.duty_cycle - self._min_duty) / self._duty_range

    @fraction.setter
    def fraction(self, value: Optional[float]) -> None:
        if value is None:
            #self._pwm_out.duty_cycle = 0  # disable the motor

            return
        if not 0.0 <= value <= 1.0:
            raise ValueError("Must be 0.0 to 1.0")
        #if self.ch > 2 and self.ch < 6:
        #    valuetmp = 1.0 - value
        #    self.duty_cycle = self._min_duty + int(valuetmp * self._duty_range)
        #else: 
        self.duty_cycle = self._min_duty + int(value * self._duty_range)
        #print(self.ch)
        #if self.ch == 0:
            #print(value)
        pca.setTarget(self.ch,int(self.duty_cycle))
        time.sleep(0.002)
        #self._pwm_out.duty_cycle = duty_cycle