|
| 1 | +#!/usr/bin/python |
| 2 | + |
| 3 | +import time |
| 4 | +import math |
| 5 | +import smbus |
| 6 | + |
| 7 | + |
| 8 | +# ============================================================================ |
| 9 | +# Raspi PCA9685 16-Channel PWM Servo Driver |
| 10 | +# ============================================================================ |
| 11 | + |
| 12 | +class PCA9685: |
| 13 | + # Registers/etc. |
| 14 | + __SUBADR1 = 0x02 |
| 15 | + __SUBADR2 = 0x03 |
| 16 | + __SUBADR3 = 0x04 |
| 17 | + __MODE1 = 0x00 |
| 18 | + __PRESCALE = 0xFE |
| 19 | + __LED0_ON_L = 0x06 |
| 20 | + __LED0_ON_H = 0x07 |
| 21 | + __LED0_OFF_L = 0x08 |
| 22 | + __LED0_OFF_H = 0x09 |
| 23 | + __ALLLED_ON_L = 0xFA |
| 24 | + __ALLLED_ON_H = 0xFB |
| 25 | + __ALLLED_OFF_L = 0xFC |
| 26 | + __ALLLED_OFF_H = 0xFD |
| 27 | + |
| 28 | + def __init__(self, address=0x40, debug=False): |
| 29 | + self.bus = smbus.SMBus(1) |
| 30 | + self.address = address |
| 31 | + self.debug = debug |
| 32 | + if (self.debug): |
| 33 | + print("Reseting PCA9685") |
| 34 | + self.write(self.__MODE1, 0x00) |
| 35 | + |
| 36 | + def write(self, reg, value): |
| 37 | + "Writes an 8-bit value to the specified register/address" |
| 38 | + self.bus.write_byte_data(self.address, reg, value) |
| 39 | + if (self.debug): |
| 40 | + print("I2C: Write 0x%02X to register 0x%02X" % (value, reg)) |
| 41 | + |
| 42 | + def read(self, reg): |
| 43 | + "Read an unsigned byte from the I2C device" |
| 44 | + result = self.bus.read_byte_data(self.address, reg) |
| 45 | + if (self.debug): |
| 46 | + print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, result & 0xFF, reg)) |
| 47 | + return result |
| 48 | + |
| 49 | + def setPWMFreq(self, freq): |
| 50 | + "Sets the PWM frequency" |
| 51 | + prescaleval = 25000000.0 # 25MHz |
| 52 | + prescaleval /= 4096.0 # 12-bit |
| 53 | + prescaleval /= float(freq) |
| 54 | + prescaleval -= 1.0 |
| 55 | + if (self.debug): |
| 56 | + print("Setting PWM frequency to %d Hz" % freq) |
| 57 | + print("Estimated pre-scale: %d" % prescaleval) |
| 58 | + prescale = math.floor(prescaleval + 0.5) |
| 59 | + if (self.debug): |
| 60 | + print("Final pre-scale: %d" % prescale) |
| 61 | + |
| 62 | + oldmode = self.read(self.__MODE1); |
| 63 | + newmode = (oldmode & 0x7F) | 0x10 # sleep |
| 64 | + self.write(self.__MODE1, newmode) # go to sleep |
| 65 | + self.write(self.__PRESCALE, int(math.floor(prescale))) |
| 66 | + self.write(self.__MODE1, oldmode) |
| 67 | + time.sleep(0.005) |
| 68 | + self.write(self.__MODE1, oldmode | 0x80) |
| 69 | + |
| 70 | + def setPWM(self, channel, on, off): |
| 71 | + "Sets a single PWM channel" |
| 72 | + self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF) |
| 73 | + self.write(self.__LED0_ON_H + 4 * channel, on >> 8) |
| 74 | + self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF) |
| 75 | + self.write(self.__LED0_OFF_H + 4 * channel, off >> 8) |
| 76 | + if (self.debug): |
| 77 | + print("channel: %d LED_ON: %d LED_OFF: %d" % (channel, on, off)) |
| 78 | + |
| 79 | + def setServoPulse(self, channel, pulse): |
| 80 | + "Sets the Servo Pulse,The PWM frequency must be 50HZ" |
| 81 | + pulse = pulse * 4096 / 20000 # PWM frequency is 50HZ,the period is 20000us |
| 82 | + self.setPWM(channel, 0, int(pulse)) |
| 83 | + |
| 84 | + |
| 85 | +if __name__ == '__main__': |
| 86 | + |
| 87 | + pwm = PCA9685(0x40, debug=False) |
| 88 | + pwm.setPWMFreq(50) |
| 89 | + while True: |
| 90 | + # setServoPulse(2,2500) |
| 91 | + for i in range(500, 2500, 10): |
| 92 | + pwm.setServoPulse(0, i) |
| 93 | + time.sleep(0.02) |
| 94 | + |
| 95 | + for i in range(2500, 500, -10): |
| 96 | + pwm.setServoPulse(0, i) |
| 97 | + time.sleep(0.02) |
0 commit comments