ArtronShop Servo HAT (Lib)

PCA9685.py

import time
import smbus

class PCA9685:
    """
    This class provides an interface to the I2C PCA9685 PWM chip.
    The chip provides 16 PWM channels.
    All channels use the same frequency which may be set in the
    range 24 to 1526 Hz.
    If used to drive servos the frequency should normally be set
    in the range 50 to 60 Hz.
    The duty cycle for each channel may be independently set
    between 0 and 100%.
    It is also possible to specify the desired pulse width in
    microseconds rather than the duty cycle.  This may be more
    convenient when the chip is used to drive servos.
    The chip has 12 bit resolution, i.e. there are 4096 steps
    between off and full on.
    """

    _MODE1         = 0x00
    _MODE2         = 0x01
    _SUBADR1       = 0x02
    _SUBADR2       = 0x03
    _SUBADR3       = 0x04
    _PRESCALE      = 0xFE
    _LED0_ON_L     = 0x06
    _LED0_ON_H     = 0x07
    _LED0_OFF_L    = 0x08
    _LED0_OFF_H    = 0x09
    _ALL_LED_ON_L  = 0xFA
    _ALL_LED_ON_H  = 0xFB
    _ALL_LED_OFF_L = 0xFC
    _ALL_LED_OFF_H = 0xFD

    _RESTART = 1<<7
    _AI      = 1<<5
    _SLEEP   = 1<<4
    _ALLCALL = 1<<0

    _OCH     = 1<<3
    _OUTDRV  = 1<<2

    def __init__(self, bus_num=1, address=0x40):
        self.bus = smbus.SMBus(bus_num)
        self.address = address

        self._write_reg(self._MODE1, self._AI | self._ALLCALL)
        self._write_reg(self._MODE2, self._OCH | self._OUTDRV)

        time.sleep(0.0005)

        mode = self._read_reg(self._MODE1)
        self._write_reg(self._MODE1, mode & ~self._SLEEP)

        time.sleep(0.0005)

        self.set_duty_cycle(-1, 0)
        self.set_frequency(46.4)

    def get_frequency(self):
        return self._frequency

    def set_frequency(self, frequency):
        prescale = int(round(25000000.0 / (4095.0 * frequency)) - 1)

        if prescale < 3:
            prescale = 3
        elif prescale > 255:
            prescale = 255

        mode = self._read_reg(self._MODE1);
        self._write_reg(self._MODE1, (mode & ~self._SLEEP) | self._SLEEP)
        self._write_reg(self._PRESCALE, prescale)
        self._write_reg(self._MODE1, mode)

        time.sleep(0.0005)

        self._write_reg(self._MODE1, mode | self._RESTART)

        self._frequency = (25000000.0 / 4096.0) / (prescale + 1)
        self._pulse_width = (1000000.0 / self._frequency)

    def set_duty_cycle(self, channel, percent):
        steps = int(round(percent * (4096.0 / 100.0)))

        if steps < 0:
            on = 0
            off = 4096
        elif steps > 4095:
            on = 4096
            off = 0
        else:
            on = 0
            off = steps

        reg = self._ALL_LED_ON_L
        if (channel >= 0) and (channel <= 15):
            reg = self._LED0_ON_L+4 * channel

        self.bus.write_i2c_block_data(self.address, reg, [
            on & 0xFF, 
            on >> 8, 
            off & 0xFF, 
            off >> 8
        ])

    def set_pulse_width(self, channel, width):
        self.set_duty_cycle(channel, (float(width) / self._pulse_width) * 100.0)
    
    def set_angle(self, channel, value):
        offset = 500
        # servo.set_pulse_width(channel, (2000 / 180 * value) + offset)
        on = 0
        off = int((4095 / 20000) * ((2000 / 180 * value) + offset))
        reg = (self._LED0_ON_L+4 * channel) if (channel >= 0) and (channel <= 15) else self._ALL_LED_ON_L
            
        self.bus.write_i2c_block_data(self.address, reg, [
            on & 0xFF, 
            on >> 8, 
            off & 0xFF, 
            off >> 8
        ])

    def cancel(self):
        self.set_duty_cycle(-1, 0)
        self.bus.close()

    def _write_reg(self, reg, byte):
        self.bus.write_byte_data(self.address, reg, byte)

    def _read_reg(self, reg):
        return self.bus.read_byte_data(self.address, reg)