From df74fc2eb4c700fdcf2f5c98111d0bd306336bb9 Mon Sep 17 00:00:00 2001
From: shrkey <barry@shrkey.com>
Date: Wed, 6 Apr 2016 21:08:20 +0100
Subject: [PATCH] Updated library

---
 Adafruit_PWM_Servo_Driver.py | 57 ++++++++++++++++++++++++++++++++++--
 dw640HAT.py                  |  3 --
 2 files changed, 54 insertions(+), 6 deletions(-)

diff --git a/Adafruit_PWM_Servo_Driver.py b/Adafruit_PWM_Servo_Driver.py
index 35c993c..6cf4566 100644
--- a/Adafruit_PWM_Servo_Driver.py
+++ b/Adafruit_PWM_Servo_Driver.py
@@ -50,13 +50,13 @@ class PWM :
     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):
+  def setPWMFreq(self, freq, correctionFactor=1.0):
     "Sets the PWM frequency"
     prescaleval = 25000000.0    # 25MHz
     prescaleval /= 4096.0       # 12-bit
@@ -65,7 +65,7 @@ class PWM :
     if (self.debug):
       print "Setting PWM frequency to %d Hz" % freq
       print "Estimated pre-scale: %d" % prescaleval
-    prescale = math.floor(prescaleval + 0.5)
+    prescale = round(prescaleval * correctionFactor + 0.5)
     if (self.debug):
       print "Final pre-scale: %d" % prescale
 
@@ -77,6 +77,57 @@ class PWM :
     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
+    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 * 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 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 "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 "Calculated Frequency: %d" % calcfreq
+
+    return calcfreq
+
   def setPWM(self, channel, on, off):
     "Sets a single PWM channel"
     self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF)
diff --git a/dw640HAT.py b/dw640HAT.py
index f41afde..b09e847 100644
--- a/dw640HAT.py
+++ b/dw640HAT.py
@@ -249,11 +249,8 @@ class dw_MotorCONTROL:
                 self._pwm.setPWMFreq(self._frequency)
                 # Just gonna default to high for now
                 GPIO.setmode(GPIO.BCM)
-
                 GPIO.setwarnings(False)
-                
                 GPIO.setup(17, GPIO.OUT)
-
                 GPIO.output(17, GPIO.HIGH)
 
                 self.motors = [ dw_DCMotor(self, m) for m in range(6) ]