adding pwm sample

3 files changed, 284 insertions, 0 deletions

diff --git a/python/examples/pnPWM/Adafruit_I2C.py b/python/examples/pnPWM/Adafruit_I2C.py
new file mode 100755
index 0000000..ba81def
--- /dev/null
+++ b/python/examples/pnPWM/Adafruit_I2C.py
@@ -0,0 +1,154 @@
+#!/usr/bin/python
+
+import smbus
+
+# ===========================================================================
+# Adafruit_I2C Class
+# ===========================================================================
+
+class Adafruit_I2C(object):
+
+  @staticmethod
+  def getPiRevision():
+    "Gets the version number of the Raspberry Pi board"
+    # Courtesy quick2wire-python-api
+    # https://github.com/quick2wire/quick2wire-python-api
+    # Updated revision info from: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
+    try:
+      with open('/proc/cpuinfo','r') as f:
+        for line in f:
+          if line.startswith('Revision'):
+            return 1 if line.rstrip()[-1] in ['2','3'] else 2
+    except:
+      return 0
+
+  @staticmethod
+  def getPiI2CBusNumber():
+    # Gets the I2C bus number /dev/i2c#
+    return 1 if Adafruit_I2C.getPiRevision() > 1 else 0
+
+  def __init__(self, address, busnum=-1, debug=False):
+    self.address = address
+    # By default, the correct I2C bus is auto-detected using /proc/cpuinfo
+    # Alternatively, you can hard-code the bus version below:
+    # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's)
+    # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's)
+    self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber())
+    self.debug = debug
+
+  def reverseByteOrder(self, data):
+    "Reverses the byte order of an int (16-bit) or long (32-bit) value"
+    # Courtesy Vishal Sapre
+    byteCount = len(hex(data)[2:].replace('L','')[::2])
+    val       = 0
+    for i in range(byteCount):
+      val    = (val << 8) | (data & 0xff)
+      data >>= 8
+    return val
+
+  def errMsg(self):
+    print "Error accessing 0x%02X: Check your I2C address" % self.address
+    return -1
+
+  def write8(self, reg, value):
+    "Writes an 8-bit value to the specified register/address"
+    try:
+      self.bus.write_byte_data(self.address, reg, value)
+      if self.debug:
+        print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg)
+    except IOError, err:
+      return self.errMsg()
+
+  def write16(self, reg, value):
+    "Writes a 16-bit value to the specified register/address pair"
+    try:
+      self.bus.write_word_data(self.address, reg, value)
+      if self.debug:
+        print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" %
+         (value, reg, reg+1))
+    except IOError, err:
+      return self.errMsg()
+
+  def writeRaw8(self, value):
+    "Writes an 8-bit value on the bus"
+    try:
+      self.bus.write_byte(self.address, value)
+      if self.debug:
+        print "I2C: Wrote 0x%02X" % value
+    except IOError, err:
+      return self.errMsg()
+
+  def writeList(self, reg, list):
+    "Writes an array of bytes using I2C format"
+    try:
+      if self.debug:
+        print "I2C: Writing list to register 0x%02X:" % reg
+        print list
+      self.bus.write_i2c_block_data(self.address, reg, list)
+    except IOError, err:
+      return self.errMsg()
+
+  def readList(self, reg, length):
+    "Read a list of bytes from the I2C device"
+    try:
+      results = self.bus.read_i2c_block_data(self.address, reg, length)
+      if self.debug:
+        print ("I2C: Device 0x%02X returned the following from reg 0x%02X" %
+         (self.address, reg))
+        print results
+      return results
+    except IOError, err:
+      return self.errMsg()
+
+  def readU8(self, reg):
+    "Read an unsigned byte from the I2C device"
+    try:
+      result = self.bus.read_byte_data(self.address, reg)
+      if self.debug:
+        print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
+         (self.address, result & 0xFF, reg))
+      return result
+    except IOError, err:
+      return self.errMsg()
+
+  def readS8(self, reg):
+    "Reads a signed byte from the I2C device"
+    try:
+      result = self.bus.read_byte_data(self.address, reg)
+      if result > 127: result -= 256
+      if self.debug:
+        print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
+         (self.address, result & 0xFF, reg))
+      return result
+    except IOError, err:
+      return self.errMsg()
+
+  def readU16(self, reg, little_endian=True):
+    "Reads an unsigned 16-bit value from the I2C device"
+    try:
+      result = self.bus.read_word_data(self.address,reg)
+      # Swap bytes if using big endian because read_word_data assumes little 
+      # endian on ARM (little endian) systems.
+      if not little_endian:
+        result = ((result << 8) & 0xFF00) + (result >> 8)
+      if (self.debug):
+        print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg)
+      return result
+    except IOError, err:
+      return self.errMsg()
+
+  def readS16(self, reg, little_endian=True):
+    "Reads a signed 16-bit value from the I2C device"
+    try:
+      result = self.readU16(reg,little_endian)
+      if result > 32767: result -= 65536
+      return result
+    except IOError, err:
+      return self.errMsg()
+
+if __name__ == '__main__':
+  try:
+    bus = Adafruit_I2C(address=0)
+    print "Default I2C bus is accessible"
+  except:
+    print "Error accessing default I2C bus"
diff --git a/python/examples/pnPWM/Adafruit_PWM_Servo_Driver.py b/python/examples/pnPWM/Adafruit_PWM_Servo_Driver.py
new file mode 100644
index 0000000..35c993c
--- /dev/null
+++ b/python/examples/pnPWM/Adafruit_PWM_Servo_Driver.py
@@ -0,0 +1,92 @@
+#!/usr/bin/python
+
+import time
+import math
+from Adafruit_I2C import Adafruit_I2C
+
+# ============================================================================
+# Adafruit PCA9685 16-Channel PWM Servo Driver
+# ============================================================================
+
+class PWM :
+  # Registers/etc.
+  __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
+
+  # Bits
+  __RESTART            = 0x80
+  __SLEEP              = 0x10
+  __ALLCALL            = 0x01
+  __INVRT              = 0x10
+  __OUTDRV             = 0x04
+
+  general_call_i2c = Adafruit_I2C(0x00)
+
+  @classmethod
+  def softwareReset(cls):
+    "Sends a software reset (SWRST) command to all the servo drivers on the bus"
+    cls.general_call_i2c.writeRaw8(0x06)        # SWRST
+
+  def __init__(self, address=0x40, debug=False):
+    self.i2c = Adafruit_I2C(address)
+    self.i2c.debug = debug
+    self.address = address
+    self.debug = debug
+    if (self.debug):
+      print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2"
+    self.setAllPWM(0, 0)
+    self.i2c.write8(self.__MODE2, self.__OUTDRV)
+    self.i2c.write8(self.__MODE1, self.__ALLCALL)
+    time.sleep(0.005)                                       # wait for oscillator
+    
+    mode1 = self.i2c.readU8(self.__MODE1)
+    mode1 = mode1 & ~self.__SLEEP                 # wake up (reset sleep)
+    self.i2c.write8(self.__MODE1, mode1)
+    time.sleep(0.005)                             # wait for oscillator
+
+  def setPWMFreq(self, freq):
+    "Sets the PWM frequency"
+    prescaleval = 25000000.0    # 25MHz
+    prescaleval /= 4096.0       # 12-bit
+    prescaleval /= float(freq)
+    prescaleval -= 1.0
+    if (self.debug):
+      print "Setting PWM frequency to %d Hz" % freq
+      print "Estimated pre-scale: %d" % prescaleval
+    prescale = math.floor(prescaleval + 0.5)
+    if (self.debug):
+      print "Final pre-scale: %d" % prescale
+
+    oldmode = self.i2c.readU8(self.__MODE1);
+    newmode = (oldmode & 0x7F) | 0x10             # sleep
+    self.i2c.write8(self.__MODE1, newmode)        # go to sleep
+    self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
+    self.i2c.write8(self.__MODE1, oldmode)
+    time.sleep(0.005)
+    self.i2c.write8(self.__MODE1, oldmode | 0x80)
+
+  def setPWM(self, channel, on, off):
+    "Sets a single PWM channel"
+    self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF)
+    self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8)
+    self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF)
+    self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8)
+
+  def setAllPWM(self, on, off):
+    "Sets a all PWM channels"
+    self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF)
+    self.i2c.write8(self.__ALL_LED_ON_H, on >> 8)
+    self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF)
+    self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8)
diff --git a/python/examples/pnPWM/Servo_Example.py b/python/examples/pnPWM/Servo_Example.py
new file mode 100644
index 0000000..c5ca88a
--- /dev/null
+++ b/python/examples/pnPWM/Servo_Example.py
@@ -0,0 +1,38 @@
+#!/usr/bin/python
+
+from Adafruit_PWM_Servo_Driver import PWM
+import time
+
+# ===========================================================================
+# Example Code
+# ===========================================================================
+
+# Initialise the PWM device using the default address
+pwm = PWM(0x40, debug=True)
+# Note if you'd like more debug output you can instead run:
+#pwm = PWM(0x40, debug=True)
+
+servoMin = 150  # Min pulse length out of 4096
+servoMax = 600  # Max pulse length out of 4096
+
+def setServoPulse(channel, pulse):
+  pulseLength = 1000000                   # 1,000,000 us per second
+  pulseLength /= 60                       # 60 Hz
+  print "%d us per period" % pulseLength
+  pulseLength /= 4096                     # 12 bits of resolution
+  print "%d us per bit" % pulseLength
+  pulse *= 1000
+  pulse /= pulseLength
+  pwm.setPWM(channel, 0, pulse)
+
+pwm.setPWMFreq(60)                        # Set frequency to 60 Hz
+while (True):
+    for x in range(0,15) :
+        # Change speed of continuous servo on channel O
+     	pwm.setPWM(x, x, servoMin)
+ 	time.sleep(0.0001)
+  	pwm.setPWM(x, x, servoMax)
+  	time.sleep(0.0001)
+
+
+