diff --git a/CVReader.ino b/CVReader.ino
index e42d284..ed2822a 100644
--- a/CVReader.ino
+++ b/CVReader.ino
@@ -15,11 +15,7 @@
 // The filter must be enabled by calling the DCC EXParser::setFilter method, see use in setup().
  
 void myFilter(Stream & stream, byte & opcode, byte & paramCount, int p[]) {
-    switch (opcode) {
-      case 'T':  // Intercept turnout functions because I have special hardware
-              DIAG(F("\nStop messing with Turnouts!"));
-              opcode=0; // tell parssr to ignore ithis command
-              break;   
+    switch (opcode) {  
        case 'F': // Invent new command to call the new Loco Function API <F cab func 1|0>
              DIAG(F("Setting loco %d F%d %S"),p[0],p[1],p[2]?F("ON"):F("OFF"));
              DCC::setFn(p[0],p[1],p[2]==1); 
@@ -51,7 +47,7 @@ DCCEXParser  serialParser;
 
 // Try monitoring the memory
 #include "freeMemory.h"
-int minMemory;
+int minMemory=32767;
 
 void setup() {
    Serial.begin(SERIAL_BAUD_RATE);
@@ -65,8 +61,6 @@ void setup() {
    DCCEXParser::setFilter(myFilter);
 
    malloc(1);
-   minMemory=freeMemory();
-   DIAG(F("\nFree memory=%d"),minMemory);
    DIAG(F("\nReady for JMRI commands\n"));
    
 }
diff --git a/Config.h b/Config.h
index 07be53a..d06c941 100644
--- a/Config.h
+++ b/Config.h
@@ -25,7 +25,7 @@ const byte PROG_BRAKE_PIN = 10;
 const float PROG_SENSE_FACTOR=1; //  analgRead(PROG_SENSE_PIN) * PROG_SENSE_FACTOR = milliamps 
 
 // Allocations with memory implications..!
-// Base system takes approx 500 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created
+// Base system takes approx 700 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created
 const byte MAX_LOCOS=50;             
 
 #endif
diff --git a/Hardware.cpp b/Hardware.cpp
index d767a54..c276e8d 100644
--- a/Hardware.cpp
+++ b/Hardware.cpp
@@ -4,6 +4,7 @@
 #include "Config.h"
 #include "DIAG.h"
 
+
 #if defined(ARDUINO_ARCH_AVR)
     #include <DIO2.h>  // use IDE menu Tools..Manage Libraries to locate and  install DIO2
     #define WritePin digitalWrite2
@@ -52,10 +53,6 @@ void Hardware::setCallback(int duration, void (*isr)()) {
   Timer1.attachInterrupt(isr);
 }
 
-void Hardware::setServo(byte pin, int angle) {
-  // TBD
-}
-
 
 // Railcom support functions, not yet implemented
 void Hardware::setSingleCallback(int duration, void (*isr)()) {
diff --git a/Hardware.h b/Hardware.h
index 1f6e741..7b68466 100644
--- a/Hardware.h
+++ b/Hardware.h
@@ -11,6 +11,5 @@ class Hardware {
     static void setCallback(int duration,  void (*isr)());
     static void setSingleCallback(int duration,  void (*isr)());
     static void resetSingleCallback(int duration);
-    static void setServo(byte pin, int angle);
 };
 #endif
diff --git a/PWMServoDriver.cpp b/PWMServoDriver.cpp
new file mode 100644
index 0000000..8871a75
--- /dev/null
+++ b/PWMServoDriver.cpp
@@ -0,0 +1,89 @@
+
+
+/*!
+ *  @file PWMServoDriver.cpp
+ *
+ *  @mainpage Adafruit 16-channel PWM & Servo driver, based on Adafruit_PWMServoDriver
+ *
+ *  @section intro_sec Introduction
+ *
+ *  This is a library for the 16-channel PWM & Servo driver.
+ *
+ *  Designed specifically to work with the Adafruit PWM & Servo driver.
+ *  This class contains a very small subset of the Adafruit version which
+ *  is relevant to driving simple servos at 50Hz through a number of chained 
+ *  servo driver boards (ie servos 0-15 on board 0x40, 16-31 on board 0x41 etc.)
+ *  
+ *  @section author Author
+ *  Chris Harlow (TPL) 
+ *  original by Limor Fried/Ladyada (Adafruit Industries).
+ *
+ *  @section license License
+ *
+ *  BSD license, all text above must be included in any redistribution
+ */
+#include <Arduino.h>
+#include <Wire.h>
+#include "PWMServoDriver.h"
+#define DIAG_ENABLED true
+#include "DIAG.h"
+
+
+// REGISTER ADDRESSES
+const byte PCA9685_MODE1=0x00;      // Mode Register 
+const byte PCA9685_FIRST_SERVO=0x06;  /** low byte first servo register ON*/
+const byte PCA9685_PRESCALE=0xFE;     /** Prescale register for PWM output frequency */
+// MODE1 bits
+const byte MODE1_SLEEP=0x10;   /**< Low power mode. Oscillator off */
+const byte MODE1_AI=0x20;      /**< Auto-Increment enabled */
+const byte MODE1_RESTART=0x80; /**< Restart enabled */
+
+const byte PCA9685_I2C_ADDRESS=0x40;      /** First PCA9685 I2C Slave Address */
+const float FREQUENCY_OSCILLATOR=25000000.0; /** Accurate enough for our purposes  */
+const uint8_t PRESCALE_50HZ = (uint8_t)(((FREQUENCY_OSCILLATOR / (50.0 * 4096.0)) + 0.5) - 1);
+  
+/*!
+ *  @brief  Sets the PWM frequency for a chip to 50Hz for servos
+ */
+
+byte PWMServoDriver::setupFlags=0;  // boards that have been initialised
+
+void PWMServoDriver::setup(int board) {
+  if (board>3 || setupFlags & (1<<board)) return;
+  Wire.begin();
+  uint8_t i2caddr=PCA9685_I2C_ADDRESS + board;
+  //DIAG(F("\nPWMServoDriver::setup %x prescale=%d"),i2caddr,PRESCALE_50HZ); 
+  writeRegister(i2caddr,PCA9685_MODE1, MODE1_SLEEP | MODE1_AI);    
+  writeRegister(i2caddr,PCA9685_PRESCALE, PRESCALE_50HZ);  
+  writeRegister(i2caddr,PCA9685_MODE1,MODE1_AI);
+  writeRegister(i2caddr,PCA9685_MODE1,  MODE1_RESTART | MODE1_AI);
+  setupFlags|=1<<board;
+}
+
+/*!
+ *  @brief  Sets the PWM output to a servo
+ */
+void PWMServoDriver::setServo(short servoNum, uint16_t value) {
+  //DIAG(F("\nsetServo %d %d\n"),servoNum,value);
+  int board=servoNum/16; 
+  int pin=servoNum%16;
+  
+  if (board<0 | board>3) return; // safe dropout
+  setup(board); // in case not already done
+  
+  Wire.beginTransmission(PCA9685_I2C_ADDRESS + board);
+  Wire.write(PCA9685_FIRST_SERVO + 4 * pin); // 4 registers per pin
+  Wire.write(0);
+  Wire.write(0);
+  Wire.write(value);
+  Wire.write(value >> 8);
+  Wire.endTransmission();
+}
+
+void PWMServoDriver::writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d) {
+  Wire.beginTransmission(i2caddr);
+  Wire.write(hardwareRegister);
+  Wire.write(d);
+  Wire.endTransmission();
+  delay(5); // allow registers to settle before continuing
+}
diff --git a/PWMServoDriver.h b/PWMServoDriver.h
new file mode 100644
index 0000000..ca8b304
--- /dev/null
+++ b/PWMServoDriver.h
@@ -0,0 +1,20 @@
+/*!
+ *  @file PWMServoDriver.h
+ *
+ *  Used to set servo positions on an I2C bus with 1 or more PCA96685 boards.
+ */
+#ifndef PWMServoDriver_H
+#define PWMServoDriver_H
+
+
+class PWMServoDriver {
+public:
+    static void setServo(short servoNum,  uint16_t pos);
+    
+private:
+  static byte setupFlags; 
+  static void setup(int board);
+  static void writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d);
+};
+
+#endif
diff --git a/Turnouts.cpp b/Turnouts.cpp
index 47ca972..3d6fd29 100644
--- a/Turnouts.cpp
+++ b/Turnouts.cpp
@@ -2,6 +2,8 @@
 #include "EEStore.h"
 #include "StringFormatter.h"
 #include "Hardware.h"
+#include "PWMServoDriver.h"
+
  bool Turnout::activate(int n,bool state){
   Turnout * tt=get(n);
   if (tt==NULL) return false;
@@ -14,7 +16,7 @@
 void Turnout::activate(bool state) {
   if (state) data.tStatus|=STATUS_ACTIVE;
   else data.tStatus &= ~STATUS_ACTIVE;                            
-  if (data.tStatus & STATUS_PWM) Hardware::setServo(data.tStatus & STATUS_PWMPIN, (data.inactiveAngle+state?data.moveAngle:0));
+  if (data.tStatus & STATUS_PWM) PWMServoDriver::setServo(data.tStatus & STATUS_PWMPIN, (data.inactiveAngle+state?data.moveAngle:0));
      else DCC::setAccessory(data.address,data.subAddress, state);
 }
 ///////////////////////////////////////////////////////////////////////////////