From 3d5543e135ba06c1707a4a9957c58c7ac6bb09ba Mon Sep 17 00:00:00 2001
From: shrkey <barry@shrkey.com>
Date: Sun, 3 Apr 2016 14:04:44 +0100
Subject: [PATCH] Brought inline with escape code

---
 dw640HAT/Adafruit_PWM_Servo_Driver.py | 50 +++++++++++++++++++++++----
 1 file changed, 44 insertions(+), 6 deletions(-)

diff --git a/dw640HAT/Adafruit_PWM_Servo_Driver.py b/dw640HAT/Adafruit_PWM_Servo_Driver.py
index c38a56a..6cf4566 100644
--- a/dw640HAT/Adafruit_PWM_Servo_Driver.py
+++ b/dw640HAT/Adafruit_PWM_Servo_Driver.py
@@ -57,6 +57,27 @@ class PWM :
     time.sleep(0.005)                             # wait for oscillator
 
   def setPWMFreq(self, freq, correctionFactor=1.0):
+    "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 = round(prescaleval * correctionFactor + 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 setPWMFreqMin(self, freq, correctionFactor=1.0):
     "Sets the PWM frequency"
     prescaleval = 25000000.0    # 25MHz
     prescaleval /= 4096.0       # 12-bit
@@ -77,18 +98,35 @@ class PWM :
     time.sleep(0.005)
     self.i2c.write8(self.__MODE1, oldmode | 0x80)
 
-  def getPWMFreq(self):
+  def setPWMFreqMax(self, freq, correctionFactor=1.0):
+    "Sets the PWM frequency"
+    prescaleval = 25000000.0    # 25MHz
+    prescaleval /= 4096.0       # 12-bit
+    prescaleval /= float(freq)
+    prescaleval -= 1.0
     if (self.debug):
-      print "Getting PWM frequency Hz"
+      print "Setting PWM frequency to %d Hz" % freq
+      print "Estimated pre-scale: %d" % prescaleval
+    prescale = math.ceil(prescaleval * correctionFactor + 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 getPWMFreq(self):
     prescale = self.i2c.readU8(self.__PRESCALE)
-
+    calcfreq = 25000000.0 / 4096.0 / ( float(prescale) + 1 )
     if (self.debug):
       print "Got pre-scale: %d" % prescale
-      print 25000000.0 / 4096.0 / ( float(prescale) + 1 )
+      print "Calculated Frequency: %d" % calcfreq
 
-    return 25000000.0 / 4096.0 / ( float(prescale) + 1 )
-    #24576000.f / 4096.f / (data + 1)
+    return calcfreq
 
   def setPWM(self, channel, on, off):
     "Sets a single PWM channel"