From 407c63371815fcc32f5aba571602817b6ad40b26 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Fri, 21 Oct 2022 21:22:36 -0500 Subject: [PATCH] Add adafruit servokit compatible pololu library --- software/raspberry pi/adafruit_servokit.py | 190 +++++++++++++++++++++ software/raspberry pi/btserver.py | 37 ++-- software/raspberry pi/hexapod.py | 7 +- software/raspberry pi/requirements.txt | 1 + software/raspberry pi/tcpserver.py | 2 +- 5 files changed, 218 insertions(+), 19 deletions(-) create mode 100644 software/raspberry pi/adafruit_servokit.py diff --git a/software/raspberry pi/adafruit_servokit.py b/software/raspberry pi/adafruit_servokit.py new file mode 100644 index 0000000..8aee3e9 --- /dev/null +++ b/software/raspberry pi/adafruit_servokit.py @@ -0,0 +1,190 @@ +# 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 Cole Deck + +""" +`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 `_ +* `Adafruit 16-Channel 12-bit PWM/Servo Shield `_ +* `Adafruit 16-Channel PWM/Servo HAT for Raspberry Pi `_ +* `Adafruit 16-Channel PWM/Servo Bonnet for Raspberry Pi `_ + +**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 + #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 `_ 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 + ) -> 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 = 3000, max_pulse: int = 9000): + 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 + + @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[int]) -> 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") + self.duty_cycle = self._min_duty + int(value * self._duty_range) + #print(self.ch) + pca.setTarget(self.ch,self.duty_cycle) + #self._pwm_out.duty_cycle = duty_cycle \ No newline at end of file diff --git a/software/raspberry pi/btserver.py b/software/raspberry pi/btserver.py index cf24fc1..fe97562 100644 --- a/software/raspberry pi/btserver.py +++ b/software/raspberry pi/btserver.py @@ -49,28 +49,32 @@ class BluetoothServer(Thread): self.cmd_queue = out_cmd_queue - with open('/home/pi/hexapod/software/raspberry pi/config.json', 'r') as read_file: + with open('/home/cole/Downloads/hexapod/software/raspberry pi/config.json', 'r') as read_file: self.config = json.load(read_file) + try: - stream = os.popen('hciconfig hci0') - output = stream.read() - device_id = "hci0" - bt_mac = output.split("{}:".format(device_id))[1].split( - "BD Address: ")[1].split(" ")[0].strip() + stream = os.popen('hciconfig hci0') + output = stream.read() + device_id = "hci0" + bt_mac = output.split("{}:".format(device_id))[1].split( + "BD Address: ")[1].split(" ")[0].strip() - self.mac = bt_mac - self.port = 10 - self.bt_socket = socket.socket( - socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM) + self.mac = bt_mac + self.port = 10 + self.bt_socket = socket.socket( + socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM) - self.signal = self.SIG_NORMAL + self.signal = self.SIG_NORMAL + except: + pass + def run(self): try: self.bt_socket.bind((self.mac, self.port)) self.bt_socket.listen(1) print('TCP listening') - except OSError as err: + except: # OSError as err: # print('emit tcp server error') # self.status.emit(self.STOP, '') pass @@ -104,6 +108,9 @@ class BluetoothServer(Thread): break finally: - self.bt_socket.close() - self.cmd_queue.put('standby:') - print('exit') + try: + self.bt_socket.close() + self.cmd_queue.put('standby:') + print('exit') + except: + pass diff --git a/software/raspberry pi/hexapod.py b/software/raspberry pi/hexapod.py index c3ba525..1c46a09 100644 --- a/software/raspberry pi/hexapod.py +++ b/software/raspberry pi/hexapod.py @@ -28,6 +28,7 @@ # https://circuitpython.readthedocs.io/projects/servokit/en/latest/ from audioop import reverse from adafruit_servokit import ServoKit +import maestro from leg import Leg @@ -92,7 +93,7 @@ class Hexapod(Thread): self.calibration_mode = False - with open('/home/pi/hexapod/software/raspberry pi/config.json', 'r') as read_file: + with open('/home/cole/Downloads/hexapod/software/raspberry pi/config.json', 'r') as read_file: self.config = json.load(read_file) # legs' coordinates @@ -115,8 +116,8 @@ class Hexapod(Thread): self.mount_position[:, 1] = self.mount_y # Objects - self.pca_right = ServoKit(channels=16, address=0x40, frequency=120) - self.pca_left = ServoKit(channels=16, address=0x41, frequency=120) + self.pca_right = ServoKit(channels=18, address="/dev/ttyUSB0", frequency=0x0c) + self.pca_left = ServoKit(channels=18, address="/dev/ttyUSB0", frequency=0x0d) self.legs = [ # front right diff --git a/software/raspberry pi/requirements.txt b/software/raspberry pi/requirements.txt index f6c1a1f..7863599 100644 --- a/software/raspberry pi/requirements.txt +++ b/software/raspberry pi/requirements.txt @@ -1 +1,2 @@ pyserial +numpy \ No newline at end of file diff --git a/software/raspberry pi/tcpserver.py b/software/raspberry pi/tcpserver.py index dac0a8f..bd784c0 100644 --- a/software/raspberry pi/tcpserver.py +++ b/software/raspberry pi/tcpserver.py @@ -47,7 +47,7 @@ class TCPServer(Thread): self.cmd_queue = out_cmd_queue - with open('/home/pi/hexapod/software/raspberry pi/config.json', 'r') as read_file: + with open('/home/cole/Downloads/hexapod/software/raspberry pi/config.json', 'r') as read_file: self.config = json.load(read_file) self.ip = '192.168.1.127'