Version 1.16 beta 10

Added DueX2/DueX5 support
Report expansion board type in electronics string
Added Z probe type 6 (switch on E1 stop)
Support heater 6 on Duet 0.8.5 (untested, probably incomplete)
Changed M42 pin numbering, see duet3d.com wiki
Added M280 servo support
Allow separate firmware un-retract speed to be configured in M207
Allow negative extra un-retraction in M207
Allow dans ot be disabled yusing M106 P# I-1. They can then be used as
general-purpose pins (m42) or servos (M280).
Removed support for Duet WiFi prototype 1 board
Support expanson pin PB6 and use it for cooling fan tacho

23 files changed, 2037 insertions, 296 deletions

diff --git a/src/Configuration.h b/src/Configuration.h
index d9d414be..46f80bbd 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -26,11 +26,11 @@ Licence: GPL
 // Firmware name is now defined in the Pins file
 
 #ifndef VERSION
-# define VERSION "1.16beta5"
+# define VERSION "1.16beta10"
 #endif
 
 #ifndef DATE
-# define DATE "2016-10-23"
+# define DATE "2016-10-28"
 #endif
 
 #define AUTHORS "reprappro, dc42, zpl, t3p3, dnewman"
@@ -114,6 +114,7 @@ const unsigned int FirstRtdChannel = 200;			// Temperature sensor channels 200..
 const unsigned int SlowHeaterPwmFreq = 10;			// slow PWM frequency for bed and chamber heaters, compatible with DC/AC SSRs
 const unsigned int NormalHeaterPwmFreq = 250;		// normal PWM frequency used for hot ends
 const unsigned int DefaultFanPwmFreq = 250;			// increase to 25kHz using M106 command to meet Intel 4-wire PWM fan specification
+const unsigned int DefaultPinWritePwmFreq = 500;	// default PWM frequency for M42 pin writes
 
 // Default Z probe values
 
diff --git a/src/Duet/Pins_Duet.h b/src/Duet/Pins_Duet.h
index 23a1685d..9970d5b6 100644
--- a/src/Duet/Pins_Duet.h
+++ b/src/Duet/Pins_Duet.h
@@ -60,7 +60,7 @@ const float STEPPER_DAC_VOLTAGE_OFFSET = -0.025;						// Stepper motor current o
 const bool HEAT_ON = false;												// false for inverted heater (e.g. Duet v0.6), true for not (e.g. Duet v0.4)
 
 const Pin TEMP_SENSE_PINS[HEATERS] = { 5, 4, 0, 7, 8, 9, 11 };			// Analogue pin numbers
-const Pin HEAT_ON_PINS[HEATERS] = { 6, X5, X7, 7, 8, 9, NoPin };		// Heater Channel 7 (pin X17) is shared with Fan1. Only define 1 or the other
+const Pin HEAT_ON_PINS[HEATERS] = { 6, X5, X7, 7, 8, 9, X17 };			// Heater Channel 7 (pin X17) is shared with Fan1
 
 // Default thermistor parameters
 // Bed thermistor: http://uk.farnell.com/epcos/b57863s103f040/sensor-miniature-ntc-10k/dp/1299930?Ntt=129-9930
@@ -133,34 +133,17 @@ const Pin ROLAND_RTS_PIN = 17;											// Expansion pin 12, PA13_RXD1
 
 #endif
 
-// Definition of which pins we allow to be controlled using M42
-//
-// The allowed pins are these ones on the DueX4 expansion connector:
-//
-// TXD1 aka PA13 aka pin 16
-// RXD1 aka PA12 aka pin 17
-// TXD0 aka PA11 aka pin 18
-// RXD0 aka PA10 aka pin 19
-// PC4_PWML1 aka PC4 aka pin 36
-// AD14 aka PB21 aka pin 52
-// PB16 aka pin 67 (not available on Duet 0.8.5 because it is used for E1 motor current setting; could possibly allow analog output on this one)
-// RTS1 aka PA14 aka pin 23
-// TWD1 aka PB12 aka pin 20
-// TWCK1 aka PB13 aka pin 21
-
-const size_t NUM_PINS_ALLOWED = 72;
-
-#define PINS_ALLOWED {				\
-	/* pins 00-07 */	0,			\
-	/* pins 08-15 */	0,			\
-	/* pins 16-23 */	0b10111111,	\
-	/* pins 24-31 */	0,			\
-	/* pins 32-39 */	0b00010000,	\
-	/* pins 40-47 */	0,			\
-	/* pins 48-55 */	0b00010000,	\
-	/* pins 56-63 */	0,			\
-	/* pins 64-71 */	0b00001000	\
-}
+// M42 and M208 commands now use logical pin numbers, not firmware pin numbers.
+// This is the mapping from logical pins 60+ to firmware pin numbers
+const Pin SpecialPinMap[] =
+{
+	19, 18,	17, 16, 23, 	// PA10/RXD0 PA11/TXD0 PA12/RXD1 PA13/TXD1 PA14/RTS1
+	20, 21, 52, 			// PB12/TWD1 PB13/TWCK1 PB21/AD14
+	36						// PC4
+};
+
+// This next definition defines the highest one.
+const int HighestLogicalPin = 60 + ARRAY_SIZE(SpecialPinMap) - 1;		// highest logical pin number on this electronics
 
 // SAM3X Flash locations (may be expanded in the future)
 const uint32_t IAP_FLASH_START = 0x000F0000;
diff --git a/src/DuetNG/DueXn.cpp b/src/DuetNG/DueXn.cpp
new file mode 100644
index 00000000..a0f1c11b
--- /dev/null
+++ b/src/DuetNG/DueXn.cpp
@@ -0,0 +1,154 @@
+/*
+ * DueXn.cpp
+ *
+ *  Created on: 19 Oct 2016
+ *      Author: David
+ */
+
+#include "DueXn.h"
+#include "SX1509.h"
+
+namespace DuetExpansion
+{
+	static SX1509 expander;
+	static uint16_t inputMask;
+	static ExpansionBoardType boardType = ExpansionBoardType::none;
+
+	const uint8_t DueXnAddress = 0x3E;				// address of the SX1509B on the DueX0/DueX2/DueX5
+
+	const uint16_t BoardTypePins = (1u << 14) | (1u << 15);
+	const unsigned int BoardTypeShift = 14;
+	const ExpansionBoardType boardTypes[] =
+		{ ExpansionBoardType::DueX5, ExpansionBoardType::DueX2, ExpansionBoardType::DueX0, ExpansionBoardType::none };
+
+	const unsigned int Fan3Bit = 12;
+	const unsigned int Fan4Bit = 7;
+	const unsigned int Fan5Bit = 6;
+	const unsigned int Fan6Bit = 5;
+	const unsigned int Fan7Bit = 4;
+	const uint16_t AllFanBits = (1u << Fan3Bit) | (1u << Fan4Bit) | (1u << Fan5Bit) | (1u << Fan6Bit) | (1u << Fan7Bit);
+
+	const unsigned int E2StopBit = 0;
+	const unsigned int E3StopBit = 3;
+	const unsigned int E4StopBit = 2;
+	const unsigned int E5StopBit = 1;
+	const unsigned int E6StopBit = 13;
+	const uint16_t AllStopBits = (1u << E2StopBit) | (1u << E3StopBit) | (1u << E4StopBit) | (1u << E5StopBit) | (1u << E6StopBit);
+
+	const unsigned int Gpio1Bit = 11;
+	const unsigned int Gpio2Bit = 10;
+	const unsigned int Gpio3Bit = 9;
+	const unsigned int Gpio4Bit = 8;
+	const uint16_t AllGpioBits = (1u << Gpio1Bit) | (1u << Gpio2Bit) | (1u << Gpio3Bit) | (1u <<Gpio4Bit);
+
+	// Identify which expansion board (if any) is attached and initialise it
+	ExpansionBoardType Init()
+	{
+		uint8_t ret = expander.begin(DueXnAddress);
+		if (ret != 1)
+		{
+			delay(100);								// wait a little while
+			ret = expander.begin(DueXnAddress);		// do 1 retry
+		}
+
+		if (ret != 1)
+		{
+			boardType = ExpansionBoardType::none;	// no device found at that address, or a serious error
+		}
+		else
+		{
+			expander.pinModeMultiple(BoardTypePins, INPUT_PULLUP);
+			const uint16_t data = expander.digitalReadAll();
+			boardType = boardTypes[(data & BoardTypePins) >> BoardTypeShift];
+		}
+
+		if (boardType != ExpansionBoardType::none)
+		{
+			expander.pinModeMultiple(AllFanBits, OUTPUT_PWM);			// Initialise the PWM pins
+			expander.pinModeMultiple(AllStopBits, INPUT);				// Initialise the endstop inputs (no pullups because 5V-tolerant)
+			expander.pinModeMultiple(AllGpioBits, INPUT);				// Initialise the GPIO pins as inputs
+
+			// Set up the interrupt on any input change
+			inputMask = AllStopBits | AllGpioBits;
+			expander.enableInterruptMultiple(inputMask, CHANGE);
+
+			// Clear any initial interrupts
+			expander.interruptSource(true);
+		}
+
+		return boardType;
+	}
+
+	// Return the name of the expansion board, or nullptr if no expansion board
+	const char* array null GetExpansionBoardName()
+	{
+		switch(boardType)
+		{
+		case ExpansionBoardType::DueX5:
+			return "DueX5";
+		case ExpansionBoardType::DueX2:
+			return "DueX2";
+		case ExpansionBoardType::DueX0:
+			return "DueX0";
+		default:
+			return nullptr;
+		}
+	}
+
+	// Set the I/O mode of a pin
+	void SetPinMode(Pin pin, PinMode mode)
+	{
+		if (boardType != ExpansionBoardType::none)
+		{
+			if (((1 << pin) & AllGpioBits) != 0)
+			{
+				// The GPIO pins have pullup resistors to +5V, therefore we need to configure them as open drain outputs
+				switch(mode)
+				{
+				case OUTPUT_LOW:
+					mode = OUTPUT_LOW_OPEN_DRAIN;
+					break;
+				case OUTPUT_HIGH:
+					mode = OUTPUT_HIGH_OPEN_DRAIN;
+					break;
+				case OUTPUT_PWM:
+					mode = OUTPUT_PWM_OPEN_DRAIN;
+					break;
+				default:
+					break;
+				}
+			}
+			expander.pinMode(pin, mode);
+		}
+	}
+
+	// Read a pin
+	// TODO this is called from the Step ISR if the 6th axis is used, therefore if should be made more efficient.
+	// We need to use the SX15089 interrupt to read the data register using interrupts, and just retrieve that value here.
+	bool DigitalRead(Pin pin)
+	{
+		return (boardType != ExpansionBoardType::none)
+				? expander.digitalRead(pin)
+				: false;
+	}
+
+	// Write a pin
+	void DigitalWrite(Pin pin, bool high)
+	{
+		if (boardType != ExpansionBoardType::none)
+		{
+			expander.digitalWrite(pin, high);
+		}
+	}
+
+	// Set the PWM value on this pin
+	void AnalogOut(Pin pin, float pwm)
+	{
+		if (boardType != ExpansionBoardType::none)
+		{
+			expander.analogWrite(pin, (uint8_t)(constrain<float>(pwm, 0.0, 1.0) * 255));
+		}
+	}
+}			// end namespace
+
+// End
diff --git a/src/DuetNG/DueXn.h b/src/DuetNG/DueXn.h
new file mode 100644
index 00000000..312620ec
--- /dev/null
+++ b/src/DuetNG/DueXn.h
@@ -0,0 +1,32 @@
+/*
+ * DueXn.h
+ *
+ *  Created on: 19 Oct 2016
+ *      Author: David
+ */
+
+#ifndef SRC_DUETNG_DUEXN_H_
+#define SRC_DUETNG_DUEXN_H_
+
+#include "ecv.h"
+#include "Core.h"
+
+enum class ExpansionBoardType : uint8_t
+{
+	none,
+	DueX0,
+	DueX2,
+	DueX5
+};
+
+namespace DuetExpansion
+{
+	ExpansionBoardType Init();						// Initialise the device and identify which expansion board is attached
+	const char* array null GetExpansionBoardName();	// Return the name of the expansion board, or nullptr if no expansion board
+	void SetPinMode(Pin pin, PinMode mode);			// Set the I/O mode of a pin
+	bool DigitalRead(Pin pin);						// Read a pin
+	void DigitalWrite(Pin pin, bool high);			// Write a pin
+	void AnalogOut(Pin pin, float pwm);				// Set the PWM value on this pin
+};
+
+#endif /* SRC_DUETNG_DUEXN_H_ */
diff --git a/src/DuetNG/Pins_DuetNG.h b/src/DuetNG/Pins_DuetNG.h
index b553b665..f312eb2c 100644
--- a/src/DuetNG/Pins_DuetNG.h
+++ b/src/DuetNG/Pins_DuetNG.h
@@ -10,11 +10,7 @@ const size_t NumFirmwareUpdateModules = 4;			// 3 modules, plus one for manual u
 #define WIFI_WEB_FILE		"DuetWebControl.bin"
 
 // Default board type
-#ifdef PROTOTYPE_1
-#define DEFAULT_BOARD_TYPE BoardType::DuetWiFi_06
-#else
 #define DEFAULT_BOARD_TYPE BoardType::DuetWiFi_10
-#endif
 
 #define SUPPORT_ETHERNET	0					// set nonzero to support embedded web interface over Ethernet
 #define SUPPORT_INKJET		0					// set nonzero to support inkjet control
@@ -39,6 +35,8 @@ const size_t NUM_SERIAL_CHANNELS = 2;			// The number of serial IO channels (USB
 #define SERIAL_MAIN_DEVICE SerialUSB
 #define SERIAL_AUX_DEVICE Serial
 
+const Pin ExpansionStart = 200;					// Pin numbers at/above this are on the I/O expander
+
 // The numbers of entries in each array must correspond with the values of DRIVES, AXES, or HEATERS. Set values to NoPin to flag unavailability.
 
 // DRIVES
@@ -50,31 +48,20 @@ const Pin STEP_PINS[DRIVES] = { 70, 71, 72, 69, 68, 66, 65, 64, 67, 91 };
 const Pin DIRECTION_PINS[DRIVES] = { 75, 76, 77, 01, 73, 92, 86, 80, 81, 32 };
 const bool DIRECTIONS[DRIVES] = { FORWARDS, FORWARDS, FORWARDS, FORWARDS, FORWARDS, FORWARDS, FORWARDS, FORWARDS, FORWARDS, FORWARDS };	// What each axis needs to make it go forwards - defaults
 
+const Pin DueX_SG = 96;				// DueX stallguard detect pin = PE0 (was E2_STOP)
+const Pin DueX_INT = 17;			// DueX interrupt pin = PA17 (was E6_STOP)
+
 // Endstops
 // RepRapFirmware only has a single endstop per axis.
 // Gcode defines if it is a max ("high end") or min ("low end") endstop and sets if it is active HIGH or LOW.
-const Pin END_STOP_PINS[DRIVES] = { 46, 02, 93, 74, 48, 96, 97, 98, 99, 17 };
-
-// Indices for motor current digipots (if any): first 4 are for digipot 1 (on duet), second 4 for digipot 2 (on expansion board)
-#ifdef PROTOTYPE_1
-const uint8_t POT_WIPES[8] = { 1, 3, 2, 0, 1, 3, 2, 0 };
-const float SENSE_RESISTOR = 0.1;										// Stepper motor current sense resistor
-const float MAX_STEPPER_DIGIPOT_VOLTAGE = (3.3 * 2.5 / (2.7 + 2.5));	// Stepper motor current reference voltage
-const float STEPPER_DAC_VOLTAGE_RANGE = 2.02;							// Stepper motor current reference voltage for E1 if using a DAC
-const float STEPPER_DAC_VOLTAGE_OFFSET = -0.11;							// Stepper motor current offset voltage for E1 if using a DAC
-#endif
+//const Pin END_STOP_PINS[DRIVES] = { 46, 02, 93, 74, 48, 96, 97, 98, 99, 17 };
+const Pin END_STOP_PINS[DRIVES] = { 46, 02, 93, 74, 48, 200, 203, 202, 201, 213 };
 
 // HEATERS
 
 const bool HEAT_ON = false;												// false for inverted heater (e.g. Duet v0.6), true for not (e.g. Duet v0.4)
-
 const Pin TEMP_SENSE_PINS[HEATERS] = { 45, 47, 44, 61, 62, 63, 59, 18 }; // Thermistor pin numbers
-
-#ifdef PROTOTYPE_1
-const Pin HEAT_ON_PINS[HEATERS] = { 19, 20, 16, 15, 37, 40, 43, 38 };	// Heater pin numbers
-#else
 const Pin HEAT_ON_PINS[HEATERS] = { 19, 20, 16, 35, 37, 40, 43, 15 };	// Heater pin numbers (heater 7 pin TBC)
-#endif
 
 // Default thermistor parameters
 // Bed thermistor: now assuming 100K
@@ -99,14 +86,10 @@ const Pin SpiTempSensorCsPins[MaxSpiTempSensors] = { 56, 27 };
 
 #else
 
-const size_t MaxSpiTempSensors = 4;
+const size_t MaxSpiTempSensors = 8;
 
 // Digital pins the 31855s have their select lines tied to
-# ifdef PROTOTYPE_1
-const Pin SpiTempSensorCsPins[MaxSpiTempSensors] = { 24, 25, 50, 51 };	// SPI1_CS0, SPI1_CS1, CS2, CS3
-# else
-const Pin SpiTempSensorCsPins[MaxSpiTempSensors] = { 28, 50, 51, 52 };	// SPI0_CS1, SPI0_CS2, CS3, CS4
-# endif
+const Pin SpiTempSensorCsPins[MaxSpiTempSensors] = { 28, 50, 51, 52, 24, 97, 98, 99 };	// SPI0_CS1, SPI0_CS2, CS3, CS4, CS5, CS6, CS7, CS8
 
 #endif
 
@@ -116,7 +99,7 @@ const Pin ATX_POWER_PIN = 79;											// Arduino Due pin number that controls
 // Analogue pin numbers
 const Pin Z_PROBE_PIN = 33;												// AFE1_AD4/PC1 Z probe analog input
 const Pin PowerMonitorVinDetectPin = 36;								// AFE1_AD7/PC4 Vin monitor
-const Pin PowerMonitor5vDetectPin = 29;									// AFE1_AD1/PB3 Buck regulator input monitor
+const Pin PowerMonitor5vDetectPin = 29;									// AFE1_AD1/PB3 5V regulator input monitor
 
 const float PowerFailVoltageRange = 11.0 * 3.3;							// we use an 11:1 voltage divider
 
@@ -124,9 +107,9 @@ const float PowerFailVoltageRange = 11.0 * 3.3;							// we use an 11:1 voltage
 const Pin Z_PROBE_MOD_PIN = 34;											// Digital pin number to turn the IR LED on (high) or off (low) on Duet v0.6 and v1.0 (PB21)
 
 // COOLING FANS
-const size_t NUM_FANS = 3;
-const Pin COOLING_FAN_PINS[NUM_FANS] = { 55, 58, 00 };
-const Pin COOLING_FAN_RPM_PIN = 32;
+const size_t NUM_FANS = 8;
+const Pin COOLING_FAN_PINS[NUM_FANS] = { 55, 58, 00, 212, 207, 206, 205, 204 };
+const Pin COOLING_FAN_RPM_PIN = 102;									// PB6 on expansion connector
 
 // SD cards
 const size_t NumSdCards = 2;
@@ -151,31 +134,15 @@ const Pin ROLAND_RTS_PIN = xx;											// Expansion pin 12, PA13_RXD1
 
 #endif
 
-// Definition of which pins we allow to be controlled using M42
-//
-// The allowed pins are these ones on the DueX4 expansion connector:
-//TODO document these
-
-const size_t NUM_PINS_ALLOWED = 96;
-
-#if 1
-#define PINS_ALLOWED { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };	//TODO temporary!
-#else
-#define PINS_ALLOWED {				\
-	/* pins 00-07 */	0b00000000,	\
-	/* pins 08-15 */	0b00000000,	\
-	/* pins 16-23 */	0b00000110,	\
-	/* pins 24-31 */	0b10000011,	\
-	/* pins 32-39 */	0b00001001,	\
-	/* pins 40-47 */	0b00000000,	\
-	/* pins 48-55 */	0b00011000,	\
-	/* pins 56-63 */	0b00000000,	\
-	/* pins 64-71 */	0b00000000,	\
-	/* pins 72-79 */	0b00000000,	\
-	/* pins 80-87 */	0b00000000,	\
-	/* pins 88-95 */	0b00001000	\
-}
-#endif
+// M42 and M208 commands now use logical pin numbers, not firmware pin numbers.
+// This next definition defines the highest one.
+// This is the mapping from logical pins 60+ to firmware pin numbers
+const Pin SpecialPinMap[] =
+{
+	88, 97, 98, 99														// We allow CS5-CS8 to be used because few users need >4 thermocouples or RTDs
+};
+const Pin DueX5GpioPinMap[] = { 211, 210, 209, 208 };					// GPIO 1-4 on DueX5
+const int HighestLogicalPin = 100 + ARRAY_SIZE(DueX5GpioPinMap) - 1;	// highest logical pin number on this electronics
 
 // SAM4E Flash locations (may be expanded in the future)
 const uint32_t IAP_FLASH_START = 0x00470000;
diff --git a/src/DuetNG/SX1509.cpp b/src/DuetNG/SX1509.cpp
new file mode 100644
index 00000000..7837a1d5
--- /dev/null
+++ b/src/DuetNG/SX1509.cpp
@@ -0,0 +1,771 @@
+/******************************************************************************
+SparkFunSX1509.cpp
+SparkFun SX1509 I/O Expander Library Source File
+Adapted from Sparkfun SX1509 library by dc42, see header below.
+******************************************************************************/
+
+/******************************************************************************
+Jim Lindblom @ SparkFun Electronics
+Original Creation Date: September 21, 2015
+https://github.com/sparkfun/SparkFun_SX1509_Arduino_Library
+
+Here you'll find the Arduino code used to interface with the SX1509 I2C
+16 I/O expander. There are functions to take advantage of everything the
+SX1509 provides - input/output setting, writing pins high/low, reading 
+the input value of pins, LED driver utilities (blink, breath, pwm), and
+keypad engine utilites.
+
+This code is beerware; if you see me (or any other SparkFun employee) at the
+local, and you've found our code helpful, please buy us a round!
+
+Distributed as-is; no warranty is given.
+******************************************************************************/
+
+#include "Core.h"
+#include "Wire.h"
+#include "SX1509.h"
+#include "SX1509Registers.h"
+
+SX1509::SX1509()
+{
+	_clkX = 0;
+}
+
+uint8_t SX1509::begin(uint8_t address)
+{
+	// Store the received parameters into member variables
+	deviceAddress =  address;
+	
+	// Begin I2C
+	Wire.begin();
+
+	reset();
+
+	// Communication test. We'll read from two registers with different
+	// default values to verify communication.
+	unsigned int testRegisters = 0;
+	testRegisters = readWord(REG_INTERRUPT_MASK_A);	// this should return 0xFF00
+
+	// Then read a byte that should be 0x00
+	if (testRegisters == 0xFF00)
+	{
+		clock(DefaultOscDivider);
+		writeWord(REG_HIGH_INPUT_B, 0xFFFF);		// set all inputs to be 5V-tolerant
+		return 1;
+	}
+
+	return 0;
+}
+
+void SX1509::reset()
+{
+	// Software reset command sequence:
+	writeByte(REG_RESET, 0x12);
+	writeByte(REG_RESET, 0x34);
+}
+
+void SX1509::setBitsInWord(uint8_t registerAddress, uint16_t bits)
+{
+	const uint16_t regVal = readWord(registerAddress);
+	writeWord(registerAddress, regVal | bits);
+}
+
+void SX1509::clearBitsInWord(uint8_t registerAddress, uint16_t bits)
+{
+	const uint16_t regVal = readWord(registerAddress);
+	writeWord(registerAddress, regVal & (~bits));
+
+}
+
+void SX1509::pinMode(uint8_t pin, PinMode inOut)
+{
+	pinModeMultiple(1u << pin, inOut);
+}
+
+void SX1509::pinModeMultiple(uint16_t pins, PinMode inOut)
+{
+	switch (inOut)
+	{
+	case INPUT:
+		setBitsInWord(REG_DIR_B, pins);
+		clearBitsInWord(REG_PULL_UP_B, pins);
+		clearBitsInWord(REG_PULL_DOWN_B, pins);
+		break;
+
+	case INPUT_PULLUP:
+		setBitsInWord(REG_DIR_B, pins);
+		setBitsInWord(REG_PULL_UP_B, pins);
+		clearBitsInWord(REG_PULL_DOWN_B, pins);
+		break;
+
+	case INPUT_PULLDOWN:
+		setBitsInWord(REG_DIR_B, pins);
+		clearBitsInWord(REG_PULL_UP_B, pins);
+		setBitsInWord(REG_PULL_DOWN_B, pins);
+		break;
+
+	case OUTPUT_LOW:
+		clearBitsInWord(REG_PULL_UP_B, pins);
+		clearBitsInWord(REG_PULL_DOWN_B, pins);
+		clearBitsInWord(REG_DATA_B, pins);
+		clearBitsInWord(REG_OPEN_DRAIN_B, pins);
+		clearBitsInWord(REG_DIR_B, pins);
+		break;
+
+	case OUTPUT_HIGH:
+		clearBitsInWord(REG_PULL_UP_B, pins);
+		clearBitsInWord(REG_PULL_DOWN_B, pins);
+		setBitsInWord(REG_DATA_B, pins);
+		clearBitsInWord(REG_OPEN_DRAIN_B, pins);
+		clearBitsInWord(REG_DIR_B, pins);
+		break;
+
+	case OUTPUT_LOW_OPEN_DRAIN:
+		clearBitsInWord(REG_PULL_UP_B, pins);
+		clearBitsInWord(REG_PULL_DOWN_B, pins);
+		clearBitsInWord(REG_DATA_B, pins);
+		setBitsInWord(REG_OPEN_DRAIN_B, pins);
+		clearBitsInWord(REG_DIR_B, pins);
+		break;
+
+	case OUTPUT_HIGH_OPEN_DRAIN:
+		clearBitsInWord(REG_PULL_UP_B, pins);
+		clearBitsInWord(REG_PULL_DOWN_B, pins);
+		setBitsInWord(REG_DATA_B, pins);
+		setBitsInWord(REG_OPEN_DRAIN_B, pins);
+		clearBitsInWord(REG_DIR_B, pins);
+		break;
+
+	case OUTPUT_PWM:
+		ledDriverInitMultiple(pins, false, false);
+		break;
+
+	case OUTPUT_PWM_OPEN_DRAIN:
+		ledDriverInitMultiple(pins, false, true);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void SX1509::digitalWrite(uint8_t pin, bool highLow)
+{
+	if (highLow)
+	{
+		setBitsInWord(REG_DATA_B, 1u << pin);
+	}
+	else
+	{
+		clearBitsInWord(REG_DATA_B, 1u << pin);
+	}
+}
+
+bool SX1509::digitalRead(uint8_t pin)
+{
+	if (pin >= 8)
+	{
+		return (readByte(REG_DATA_B) & (1u << (pin - 8))) != 0;
+	}
+	else
+	{
+		return (readByte(REG_DATA_A) & (1u << pin)) != 0;
+	}
+}
+
+uint16_t SX1509::digitalReadAll()
+{
+	return readWord(REG_DATA_B);
+}
+
+void SX1509::ledDriverInit(uint8_t pin, bool log, bool openDrain)
+{
+	ledDriverInitMultiple(1u << pin, log, openDrain);
+}
+
+void SX1509::ledDriverInitMultiple(uint16_t pins, bool log, bool openDrain)
+{
+	if (openDrain)
+	{
+		setBitsInWord(REG_OPEN_DRAIN_B, pins);
+	}
+	else
+	{
+		clearBitsInWord(REG_OPEN_DRAIN_B, pins);
+	}
+	setBitsInWord(REG_INPUT_DISABLE_B, pins);	// disable input buffer
+	clearBitsInWord(REG_PULL_UP_B, pins);		// disable pullup
+	clearBitsInWord(REG_PULL_DOWN_B, pins);		// disable pulldown
+	clearBitsInWord(REG_DIR_B, pins);			// set as an output
+	
+
+	// Configure LED driver clock and mode (REG_MISC)
+	uint8_t tempByte = readByte(REG_MISC);
+	if (log)
+	{
+		tempByte |= (1u << 7) | (1u << 3);		// set logarithmic mode bank B and A
+	}
+	else
+	{
+		tempByte &= ~((1u << 7) | (1u << 3));	// set linear mode bank B and A
+	}
+	writeByte(REG_MISC, tempByte);
+	
+	// Enable LED driver operation (REG_LED_DRIVER_ENABLE)
+	setBitsInWord(REG_LED_DRIVER_ENABLE_B, pins);
+	
+	// Set REG_DATA bit low ~ LED driver started
+	clearBitsInWord(REG_DATA_B, pins);
+}
+
+void SX1509::analogWrite(uint8_t pin, uint8_t iOn)
+{
+	// Write the on intensity of pin
+	// Linear mode: Ion = iOn
+	// Log mode: Ion = f(iOn)
+	// We use the pins as active-high outputs to drive the fan mosfets, not to sink LED current directly.
+	// This means that we need to invert the intensity, and log mode doesn't make sense.
+	writeByte(REG_I_ON[pin], ~iOn);
+}
+
+void SX1509::blink(uint8_t pin, unsigned long tOn, unsigned long tOff, uint8_t onIntensity, uint8_t offIntensity)
+{
+	const uint8_t onReg = calculateLEDTRegister(tOn);
+	const uint8_t offReg = calculateLEDTRegister(tOff);
+	
+	setupBlink(pin, onReg, offReg, onIntensity, offIntensity, 0, 0, false, false);
+}
+
+void SX1509::breathe(uint8_t pin, unsigned long tOn, unsigned long tOff, unsigned long rise, unsigned long fall, uint8_t onInt, uint8_t offInt, bool log, bool openDrain)
+{
+	offInt = constrain<uint8_t>(offInt, 0, 7);
+	
+	const uint8_t onReg = calculateLEDTRegister(tOn);
+	const uint8_t offReg = calculateLEDTRegister(tOff);
+	
+	const uint8_t riseTime = calculateSlopeRegister(rise, onInt, offInt);
+	const uint8_t fallTime = calculateSlopeRegister(fall, onInt, offInt);
+	
+	setupBlink(pin, onReg, offReg, onInt, offInt, riseTime, fallTime, log, openDrain);
+}
+
+void SX1509::setupBlink(uint8_t pin, uint8_t tOn, uint8_t tOff, uint8_t onIntensity, uint8_t offIntensity, uint8_t tRise, uint8_t tFall, bool log, bool openDrain)
+{
+	ledDriverInit(pin, log, openDrain);
+	
+	// Keep parameters within their limits:
+	tOn &= 0x1F;	// tOn should be a 5-bit value
+	tOff &= 0x1F;	// tOff should be a 5-bit value
+	offIntensity &= 0x07;
+	// Write the time on
+	// 1-15:  TON = 64 * tOn * (255/ClkX)
+	// 16-31: TON = 512 * tOn * (255/ClkX)
+	writeByte(REG_T_ON[pin], tOn);
+	
+	// Write the time/intensity off register
+	// 1-15:  TOFF = 64 * tOff * (255/ClkX)
+	// 16-31: TOFF = 512 * tOff * (255/ClkX)
+	// linear Mode - IOff = 4 * offIntensity
+	// log mode - Ioff = f(4 * offIntensity)
+	writeByte(REG_OFF[pin], (tOff << 3) | offIntensity);
+	
+	// Write the on intensity:
+	writeByte(REG_I_ON[pin], onIntensity);
+	
+	// Prepare tRise and tFall
+	tRise &= 0x1F;	// tRise is a 5-bit value
+	tFall &= 0x1F;	// tFall is a 5-bit value
+	
+	// Write regTRise
+	// 0: Off
+	// 1-15:  TRise =      (regIOn - (4 * offIntensity)) * tRise * (255/ClkX)
+	// 16-31: TRise = 16 * (regIOn - (4 * offIntensity)) * tRise * (255/ClkX)
+	if (REG_T_RISE[pin] != 0xFF)
+	{
+		writeByte(REG_T_RISE[pin], tRise);
+	}
+
+	// Write regTFall
+	// 0: off
+	// 1-15:  TFall =      (regIOn - (4 * offIntensity)) * tFall * (255/ClkX)
+	// 16-31: TFall = 16 * (regIOn - (4 * offIntensity)) * tFall * (255/ClkX)
+	if (REG_T_FALL[pin] != 0xFF)
+	{
+		writeByte(REG_T_FALL[pin], tFall);
+	}
+}
+
+void SX1509::keypad(uint8_t rows, uint8_t columns, unsigned int sleepTime, uint8_t scanTime, uint8_t debounceTime)
+{
+	// Set regDir 0:7 outputs, 8:15 inputs:
+	uint16_t tempWord = readWord(REG_DIR_B);
+	for (uint8_t i = 0; i<rows; i++)
+	{
+		tempWord &= ~(1 << i);
+	}
+	for (uint8_t i = 8; i < (columns * 2); i++)
+	{
+		tempWord |= (1u << i);
+	}
+	writeWord(REG_DIR_B, tempWord);
+	
+	// Set regOpenDrain on 0:7:
+	uint8_t tempByte = readByte(REG_OPEN_DRAIN_A);
+	for (uint8_t i = 0; i < rows; i++)
+	{
+		tempByte |= (1u << i);
+	}
+	writeByte(REG_OPEN_DRAIN_A, tempByte);
+	
+	// Set regPullUp on 8:15:
+	tempByte = readByte(REG_PULL_UP_B);
+	for (uint8_t i=0; i < columns; i++)
+	{
+		tempByte |= (1u << i);
+	}
+	writeByte(REG_PULL_UP_B, tempByte);
+	
+	// Debounce Time must be less than scan time
+	debounceTime = constrain<uint8_t>(debounceTime, 1, 64);
+	scanTime = constrain<uint8_t>(scanTime, 1, 128);
+	if (debounceTime >= scanTime)
+	{
+		debounceTime = scanTime >> 1; // Force debounceTime to be less than scanTime
+	}
+	debounceKeypad(debounceTime, rows, columns);
+	
+	// Calculate scanTimeBits, based on scanTime
+	uint8_t scanTimeBits = 0;
+	for (uint8_t i = 7; i > 0; i--)
+	{
+		if ((scanTime & (1u << i)) != 0)
+		{
+			scanTimeBits = i;
+			break;
+		}
+	}
+	
+	// Calculate sleepTimeBits, based on sleepTime
+	uint8_t sleepTimeBits = 0;
+	if (sleepTime != 0)
+	{
+		for (uint8_t i = 7; i != 0; i--)
+		{
+			if ((sleepTime & (1u << (i + 6))) != 0)
+			{
+				sleepTimeBits = i;
+				break;
+			}
+		}
+		// If sleepTime was non-zero, but less than 128, 
+		// assume we wanted to turn sleep on, set it to minimum:
+		if (sleepTimeBits == 0)
+		{
+			sleepTimeBits = 1;
+		}
+	}
+	
+	// RegKeyConfig1 sets the auto sleep time and scan time per row
+	sleepTimeBits = (sleepTimeBits & 0b111)<<4;	
+	scanTimeBits &= 0b111;	// Scan time is bits 2:0
+	tempByte = sleepTime | scanTimeBits;
+	writeByte(REG_KEY_CONFIG_1, tempByte);
+	
+	// RegKeyConfig2 tells the SX1509 how many rows and columns we've got going
+	rows = (rows - 1) & 0b111;	// 0 = off, 0b001 = 2 rows, 0b111 = 8 rows, etc.
+	columns = (columns - 1) & 0b111;	// 0b000 = 1 column, ob111 = 8 columns, etc.
+	writeByte(REG_KEY_CONFIG_2, (rows << 3) | columns);
+}
+
+uint16_t SX1509::readKeypad()
+{
+	return ~readWord(REG_KEY_DATA_1);
+}
+
+uint8_t SX1509::getRow(uint16_t keyData)
+{
+	const uint8_t rowData = uint8_t(keyData & 0x00FF);
+	
+	for (uint8_t i = 0; i < 8; i++)
+	{
+		if (rowData & (1u << i))
+		{
+			return i;
+		}
+	}
+	return 0;
+}
+
+uint8_t SX1509::getCol(uint16_t keyData)
+{
+	const uint8_t colData = uint8_t((keyData & 0xFF00) >> 8);
+	
+	for (uint8_t i = 0; i < 8; i++)
+	{
+		if (colData & (1u << i))
+		{
+			return i;
+		}
+	}
+	return 0;
+	
+}
+
+void SX1509::debounceConfig(uint8_t configValue)
+{
+	// First make sure clock is configured
+	uint8_t tempByte = readByte(REG_MISC);
+	if ((tempByte & 0x70) == 0)
+	{
+		tempByte |= (1<<4);	// Just default to no divider if not set
+		writeByte(REG_MISC, tempByte);
+	}
+	tempByte = readByte(REG_CLOCK);
+	if ((tempByte & 0x60) == 0)
+	{
+		tempByte |= (1<<6);	// default to internal osc.
+		writeByte(REG_CLOCK, tempByte);
+	}
+	
+	configValue &= 0b111;	// 3-bit value
+	writeByte(REG_DEBOUNCE_CONFIG, configValue);
+}
+
+void SX1509::debounceTime(uint8_t time)
+{
+	// Debounce time-to-uint8_t map: (assuming fOsc = 2MHz)
+	// 0: 0.5ms		1: 1ms
+	// 2: 2ms		3: 4ms
+	// 4: 8ms		5: 16ms
+	// 6: 32ms		7: 64ms
+	// 2^(n-1)
+	uint8_t configValue = 0;
+	// We'll check for the highest set bit position, 
+	// and use that for debounceConfig
+	for (int i = 7; i >= 0; i--)
+	{
+		if ((time & (1u << i)) != 0)
+		{
+			configValue = i + 1;
+			break;
+		}
+	}
+	configValue = constrain<uint8_t>(configValue, 0, 7);
+	
+	debounceConfig(configValue);
+}
+
+void SX1509::debouncePin(uint8_t pin)
+{
+	unsigned int debounceEnable = readWord(REG_DEBOUNCE_ENABLE_B);
+	debounceEnable |= (1<<pin);
+	writeWord(REG_DEBOUNCE_ENABLE_B, debounceEnable);
+}
+
+void SX1509::debounceKeypad(uint8_t time, uint8_t numRows, uint8_t numCols)
+{
+	// Set up debounce time:
+	debounceTime(time);
+	
+	// Set up debounce pins:
+	for (uint8_t i = 0; i < numRows; i++)
+	{
+		debouncePin(i);
+	}
+	for (uint8_t i = 0; i < (8 + numCols); i++)
+	{
+		debouncePin(i);
+	}
+}
+
+void SX1509::enableInterrupt(uint8_t pin, uint8_t riseFall)
+{
+	enableInterruptMultiple(1u << pin, riseFall);
+}
+
+void SX1509::enableInterruptMultiple(uint16_t pins, uint8_t riseFall)
+{
+	// Set REG_SENSE_XXX
+	// Sensitivity is set as follows:
+	// 00: None
+	// 01: Rising
+	// 10: Falling
+	// 11: Both
+	uint8_t sensitivity;
+	switch (riseFall)
+	{
+	case CHANGE:
+		sensitivity = 0b11;
+		break;
+	case FALLING:
+		sensitivity = 0b10;
+		break;
+	case RISING:
+		sensitivity = 0b01;
+		break;
+	default:
+		sensitivity = 0;
+	}
+
+	uint32_t pinMask = readDword(REG_SENSE_HIGH_B);
+	for (unsigned int i = 0; i < 16; ++i)
+	{
+		if ((pins & (1 << i)) != 0)
+		{
+			pinMask &= ~(0x03 << (2 * i));
+			pinMask |= (sensitivity << (2 * i));
+		}
+	}
+
+	writeDword(REG_SENSE_HIGH_B, pinMask);
+
+	clearBitsInWord(REG_INTERRUPT_MASK_B, pins);
+}
+
+uint16_t SX1509::interruptSource(bool clear)
+{
+	const uint16_t intSource = readWord(REG_INTERRUPT_SOURCE_B);
+	if (clear)
+	{
+		writeWord(REG_INTERRUPT_SOURCE_B, 0xFFFF);	// Clear interrupts
+	}
+	return intSource;
+}
+
+bool SX1509::checkInterrupt(uint8_t pin)
+{
+	return (interruptSource(false) & (1u << pin)) != 0;
+}
+
+void SX1509::clock(uint8_t oscDivider)
+{
+	// RegClock constructed as follows:
+	//	6:5 - Oscillator frequency source
+	//		00: off, 01: external input, 10: internal 2MHz, 1: reserved
+	//	4 - OSCIO pin function
+	//		0: input, 1 output
+	//	3:0 - Frequency of oscout pin
+	//		0: LOW, 0xF: high, else fOSCOUT = FoSC/(2^(RegClock[3:0]-1))
+	writeByte(REG_CLOCK, (2u << 5) | (1u << 4) | (1u << 0));	// internal 2MHz oscillator, OSCIO outputs at that frequency
+	
+	// Config RegMisc[6:4] with oscDivider
+	// 0: off, else ClkX = fOSC / (2^(RegMisc[6:4] - 1))
+	oscDivider = constrain<uint8_t>(oscDivider, 1, 7);
+	_clkX = 2000000.0 / (1u << (oscDivider - 1u));			// update private clock variable
+	uint8_t regMisc = readByte(REG_MISC);
+	regMisc &= ~((0b111 << 4) | (1u << 1) | (1u << 0));		// clear clock divider bits, auto-increment is enabled, clear interrupt on register read
+	regMisc |= oscDivider << 4;
+	writeByte(REG_MISC, regMisc);
+}
+
+uint8_t SX1509::calculateLEDTRegister(int ms)
+{
+	if (_clkX == 0)
+	{
+		return 0;
+	}
+	
+	int regOn1 = (int)(((float)ms / 1000.0) / (64.0 * 255.0 / (float) _clkX));
+	int regOn2 = regOn1 / 8;
+	regOn1 = constrain<int>(regOn1, 1, 15);
+	regOn2 = constrain<int>(regOn2, 16, 31);
+	
+	const float timeOn1 = 64.0 * regOn1 * 255.0 / _clkX * 1000.0;
+	const float timeOn2 = 512.0 * regOn2 * 255.0 / _clkX * 1000.0;
+
+	return (abs(timeOn1 - ms) < abs(timeOn2 - ms)) ? regOn1 : regOn2;
+}
+
+uint8_t SX1509::calculateSlopeRegister(int ms, uint8_t onIntensity, uint8_t offIntensity)
+{
+	if (_clkX == 0)
+	{
+		return 0;
+	}
+	
+	const float tFactor = ((float) onIntensity - (4.0 * (float)offIntensity)) * 255.0 / (float) _clkX;
+	const float timeS = float(ms) / 1000.0;
+	
+	int regSlope1 = timeS / tFactor;
+	int regSlope2 = regSlope1 / 16;
+	
+	regSlope1 = constrain<int>(regSlope1, 1, 15);
+	regSlope2 = constrain<int>(regSlope2, 16, 31);
+
+	const float regTime1 = regSlope1 * tFactor * 1000.0;
+	const float regTime2 = 16 * regTime1;
+
+	return (abs(regTime1 - ms) < abs(regTime2 - ms)) ? regSlope1 : regSlope2;
+}
+
+// readByte(uint8_t registerAddress)
+//	This function reads a single uint8_t located at the registerAddress register.
+//	- deviceAddress should already be set by the constructor.
+//	- Return value is the uint8_t read from registerAddress
+//		- Currently returns 0 if communication has timed out
+uint8_t SX1509::readByte(uint8_t registerAddress)
+{
+	unsigned int timeout = ReceiveTimeout;
+
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(registerAddress);
+	Wire.endTransmission();
+	Wire.requestFrom(deviceAddress, (uint8_t) 1);
+
+	while ((Wire.available() < 1) && (timeout != 0))
+	{
+		timeout--;
+	}
+		
+	if (timeout == 0)
+	{
+		return 0;
+	}
+
+	return Wire.read();
+}
+
+// readWord(uint8_t registerAddress)
+//	This function will read a two-uint8_t word beginning at registerAddress
+//	- A 16-bit unsigned int will be returned.
+//		- The msb of the return value will contain the value read from registerAddress
+//		- The lsb of the return value will contain the value read from registerAddress + 1
+uint16_t SX1509::readWord(uint8_t registerAddress)
+{
+	unsigned int timeout = ReceiveTimeout * 2;
+
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(registerAddress);
+	Wire.endTransmission();
+	Wire.requestFrom(deviceAddress, (uint8_t) 2);
+
+	while ((Wire.available() < 2) && (timeout != 0))
+	{
+		timeout--;
+	}
+		
+	if (timeout == 0)
+	{
+		return 0;
+	}
+	
+	uint16_t msb = (Wire.read() & 0x00FF) << 8;
+	uint16_t lsb = (Wire.read() & 0x00FF);
+	return msb | lsb;
+}
+
+// readDword(uint8_t registerAddress)
+//	This function will read a two-uint8_t word beginning at registerAddress
+//	- A 32-bit unsigned int will be returned.
+//		- The msb of the return value will contain the value read from registerAddress
+//		- The lsb of the return value will contain the value read from registerAddress + 1
+uint32_t SX1509::readDword(uint8_t registerAddress)
+{
+	unsigned int timeout = ReceiveTimeout * 2;
+
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(registerAddress);
+	Wire.endTransmission();
+	Wire.requestFrom(deviceAddress, (uint8_t) 4);
+
+	while ((Wire.available() < 4) && (timeout != 0))
+	{
+		timeout--;
+	}
+
+	if (timeout == 0)
+	{
+		return 0;
+	}
+
+	uint32_t rslt = Wire.read() & 0x00FF;
+	rslt <<= 8;
+	rslt |= (Wire.read() & 0x00FF);
+	rslt <<= 8;
+	rslt |= (Wire.read() & 0x00FF);
+	rslt <<= 8;
+	rslt |= (Wire.read() & 0x00FF);
+	return rslt;
+}
+
+// readBytes(uint8_t firstRegisterAddress, uint8_t * destination, uint8_t length)
+//	This function reads a series of bytes incrementing from a given address
+//	- firstRegsiterAddress is the first address to be read
+//	- destination is an array of bytes where the read values will be stored into
+//	- length is the number of bytes to be read
+//	- No return value.
+void SX1509::readBytes(uint8_t firstRegisterAddress, uint8_t * destination, uint8_t length)
+{
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(firstRegisterAddress);
+	Wire.endTransmission();
+	Wire.requestFrom(deviceAddress, length);
+	
+	while (Wire.available() < length) { }
+	
+	for (uint8_t i = 0; i < length; i++)
+	{
+		destination[i] = Wire.read();
+	}
+}
+
+// writeByte(uint8_t registerAddress, uint8_t writeValue)
+//	This function writes a single uint8_t to a single register on the SX509.
+//	- writeValue is written to registerAddress
+//	- deviceAddres should already be set from the constructor
+//	- No return value.
+void SX1509::writeByte(uint8_t registerAddress, uint8_t writeValue)
+{
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(registerAddress);
+	Wire.write(writeValue);
+	Wire.endTransmission();
+}
+
+// writeWord(uint8_t registerAddress, uint16_t writeValue)
+//	This function writes a two-uint8_t word to registerAddress and registerAddress + 1
+//	- the upper uint8_t of writeValue is written to registerAddress
+//		- the lower uint8_t of writeValue is written to registerAddress + 1
+//	- No return value.
+void SX1509::writeWord(uint8_t registerAddress, uint16_t writeValue)
+{
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(registerAddress);
+	Wire.write((uint8_t)(writeValue >> 8));
+	Wire.write((uint8_t)writeValue);
+	Wire.endTransmission();
+}
+
+// writeDword(uint8_t registerAddress, uint32_t writeValue)
+//	This function writes a four-uint8_t word to registerAddress .. registerAddress + 3, msb first
+//	- No return value.
+void SX1509::writeDword(uint8_t registerAddress, uint32_t writeValue)
+{
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(registerAddress);
+	Wire.write((uint8_t)(writeValue >> 24));
+	Wire.write((uint8_t)(writeValue >> 16));
+	Wire.write((uint8_t)(writeValue >> 8));
+	Wire.write((uint8_t)writeValue);
+	Wire.endTransmission();	
+}
+
+// writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length)
+//	This function writes an array of bytes, beginning at a specific address
+//	- firstRegisterAddress is the initial register to be written.
+//		- All writes following will be at incremental register addresses.
+//	- writeArray should be an array of uint8_t values to be written.
+//	- length should be the number of bytes to be written.
+//	- no return value.
+void SX1509::writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length)
+{
+	Wire.beginTransmission(deviceAddress);
+	Wire.write(firstRegisterAddress);
+	for (uint8_t i=0; i < length; i++)
+	{
+		Wire.write(writeArray[i]);
+	}
+	Wire.endTransmission();
+}
+
+// End
diff --git a/src/DuetNG/SX1509.h b/src/DuetNG/SX1509.h
new file mode 100644
index 00000000..d4a0fc53
--- /dev/null
+++ b/src/DuetNG/SX1509.h
@@ -0,0 +1,425 @@
+/******************************************************************************
+SX1509.h
+SX1509 I/O Expander Library Header File
+Adapted from Sparkfun SX1509 library (see header below) by dc42
+******************************************************************************/
+
+/******************************************************************************
+Jim Lindblom @ SparkFun Electronics
+Original Creation Date: September 21, 2015
+https://github.com/sparkfun/SparkFun_SX1509_Arduino_Library
+
+Here you'll find the Arduino code used to interface with the SX1509 I2C
+16 I/O expander. There are functions to take advantage of everything the
+SX1509 provides - input/output setting, writing pins high/low, reading 
+the input value of pins, LED driver utilities (blink, breath, pwm), and
+keypad engine utilites.
+
+This code is beerware; if you see me (or any other SparkFun employee) at the
+local, and you've found our code helpful, please buy us a round!
+
+Distributed as-is; no warranty is given.
+******************************************************************************/
+
+#ifndef SX1509_H
+#define SX1509_H
+
+#include "Core.h"
+
+const int ReceiveTimeout = 1000;			// Timeout for I2C receive
+const uint8_t DefaultOscDivider = 5;		// a clock divider of 2 ^ (5 - 1) = 16 gives a PSWM frequency of 2MHz / (16 * 255) = 488Hz
+
+class SX1509
+{
+private:
+	uint8_t deviceAddress; // I2C Address of SX1509
+
+	// Misc variables:
+	uint32_t _clkX;
+
+	// Read Functions:
+	uint8_t readByte(uint8_t registerAddress);
+	uint16_t readWord(uint8_t registerAddress);
+	uint32_t readDword(uint8_t registerAddress);
+	void readBytes(uint8_t firstRegisterAddress, uint8_t * destination, uint8_t length);
+
+	// Write functions:
+	void writeByte(uint8_t registerAddress, uint8_t writeValue);
+	void writeWord(uint8_t registerAddress, uint16_t writeValue);
+	void writeDword(uint8_t registerAddress, uint32_t writeValue);
+	void writeBytes(uint8_t firstRegisterAddress, uint8_t * writeArray, uint8_t length);
+	void setBitsInWord(uint8_t registerAddress, uint16_t bits);
+	void clearBitsInWord(uint8_t registerAddress, uint16_t bits);
+
+	// Helper functions:
+
+	// calculateLEDTRegister - Try to estimate an LED on/off duration register, 
+	// given the number of milliseconds and LED clock frequency.
+	uint8_t calculateLEDTRegister(int ms);
+
+	// calculateSlopeRegister - Try to estimate an LED rise/fall duration 
+	// register, given the number of milliseconds and LED clock frequency.
+	uint8_t calculateSlopeRegister(int ms, uint8_t onIntensity, uint8_t offIntensity);
+
+public:
+	// -----------------------------------------------------------------------------
+	// Constructor - SX1509: This function sets up the pins connected to the
+	//		SX1509, and sets up the private deviceAddress variable.
+	// -----------------------------------------------------------------------------
+	SX1509();
+
+	// -----------------------------------------------------------------------------
+	// begin(uint8_t address): This function initializes the SX1509.
+	//  	It begins the Wire library, resets the IC, and tries to read some
+	//  	registers to prove it's connected.
+	// Inputs:
+	//		- address: should be the 7-bit address of the SX1509. This should be
+	//		 one of four values - 0x3E, 0x3F, 0x70, 0x71 - all depending on what the
+	//		 ADDR0 and ADDR1 pins are set to. This variable is required.
+	// Output: Returns a 1 if communication is successful, 0 on error.
+	// -----------------------------------------------------------------------------
+	uint8_t begin(uint8_t address);
+	
+	// -----------------------------------------------------------------------------
+	// reset(): This function resets the SX1509. A software
+	//		reset writes a 0x12 then 0x34 to the REG_RESET as outlined in the
+	//		datasheet.
+	// -----------------------------------------------------------------------------
+	void reset();
+
+	// -----------------------------------------------------------------------------
+	// pinMode(uint8_t pin, PinMode inOut): This function sets one of the SX1509's 16
+	//		outputs to either an INPUT or OUTPUT.
+	//
+	//	Inputs:
+	//	 	- pin: should be a value between 0 and 15
+	//	 	- inOut: The Core INPUT and OUTPUT constants should be used for the
+	//		 inOut parameter. They do what they say!
+	// -----------------------------------------------------------------------------
+	void pinMode(uint8_t pin, PinMode inOut);
+	
+	// pinModeMultiple(uint16_t pins, PinMode inOut): This function sets several of the SX1509's 16
+	//		outputs to either an INPUT or OUTPUT.
+	//
+	//	Inputs:
+	//	 	- pin: should be a value between 0 and 15
+	//	 	- inOut: The Core INPUT and OUTPUT constants should be used for the
+	//		 inOut parameter. They do what they say!
+	// -----------------------------------------------------------------------------
+	void pinModeMultiple(uint16_t pins, PinMode inOut);
+
+	// -----------------------------------------------------------------------------
+	// digitalWrite(uint8_t pin, bool highLow): This function writes a pin to either high
+	//		or low if it's configured as an OUTPUT. If the pin is configured as an
+	//		INPUT, this method will activate either the PULL-UP	or PULL-DOWN
+	//		resistor (HIGH or LOW respectively).
+	//
+	//	Inputs:
+	//		- pin: The SX1509 pin number. Should be a value between 0 and 15.
+	//		- highLow: true for HIGH, false for LOW.
+	// -----------------------------------------------------------------------------
+	void digitalWrite(uint8_t pin, bool highLow);
+
+	// -----------------------------------------------------------------------------
+	// digitalRead(uint8_t pin): This function reads the HIGH/LOW status of a pin.
+	//		The pin should be configured as an INPUT, using the pinDir function.
+	//
+	//	Inputs:
+	//	 	- pin: The SX1509 pin to be read. should be a value between 0 and 15.
+	//  Outputs:
+	//		This function returns true if HIGH, false if LOW
+	// -----------------------------------------------------------------------------
+	bool digitalRead(uint8_t pin);
+	
+	// -----------------------------------------------------------------------------
+	// digitalReadAll(): This function reads all 16 pins.
+	// -----------------------------------------------------------------------------
+	uint16_t digitalReadAll();
+
+	// -----------------------------------------------------------------------------
+	// ledDriverInit(uint8_t pin, bool log): This function initializes LED
+	//		driving on a pin. It must be called if you want to use the pwm or blink
+	//		functions on that pin.
+	//
+	//	Inputs:
+	//		- pin: The SX1509 pin connected to an LED. Should be 0-15.
+	//		- log: selects either linear or logarithmic mode on the LED drivers
+	//			- log defaults to 0, linear mode
+	//			- currently log sets both bank A and B to the same mode
+	// -----------------------------------------------------------------------------
+	void ledDriverInit(uint8_t pin, bool log, bool openDrain);
+	
+	// -----------------------------------------------------------------------------
+	// ledDriverInitMultiple(uint16_t pins, bool log): This function initializes LED
+	//		driving on a pin. It must be called if you want to use the pwm or blink
+	//		functions on that pin.
+	//
+	//	Inputs:
+	//		- pins: The SX1509 pins connected to an LED.
+	//		- log: selects either linear or logarithmic mode on the LED drivers
+	//			- log defaults to 0, linear mode
+	//			- currently log sets both bank A and B to the same mode
+	// -----------------------------------------------------------------------------
+	void ledDriverInitMultiple(uint16_t pins, bool log, bool openDrain);
+
+	// -----------------------------------------------------------------------------
+	// analogWrite(uint8_t pin, uint8_t iOn):	This function can be used to control the intensity
+	//		of an output pin connected to an LED.
+	//
+	//	Inputs:
+	//		- pin: The SX1509 pin connecte to an LED.Should be 0-15.
+	//		- iOn: should be a 0.0-1.0 value setting the intensity of the LED
+	//			- 0.0 is completely off, 1.0 is 100% on.
+	//
+	//	Note: ledDriverInit should be called on the pin before calling this.
+	// -----------------------------------------------------------------------------
+	void analogWrite(uint8_t pin, uint8_t iOn);
+	
+	// -----------------------------------------------------------------------------
+	// setupBlink(uint8_t pin, uint8_t tOn, uint8_t tOff, uint8_t offIntensity, uint8_t tRise, uint8_t
+	//		tFall):  blink performs both the blink and breath LED driver functions.
+	//
+	// 	Inputs:
+	//  	- pin: the SX1509 pin (0-15) you want to set blinking/breathing.
+	//		- tOn: the amount of time the pin is HIGH
+	//			- This value should be between 1 and 31. 0 is off.
+	//		- tOff: the amount of time the pin is at offIntensity
+	//			- This value should be between 1 and 31. 0 is off.
+	//		- offIntensity: How dim the LED is during the off period.
+	//			- This value should be between 0 and 7. 0 is completely off.
+	//		- onIntensity: How bright the LED will be when completely on.
+	//			- This value can be between 0 (0%) and 255 (100%).
+	//		- tRise: This sets the time the LED takes to fade in.
+	//			- This value should be between 1 and 31. 0 is off.
+	//			- This value is used with tFall to make the LED breath.
+	//		- tFall: This sets the time the LED takes to fade out.
+	//			- This value should be between 1 and 31. 0 is off.
+	// 	 Notes:
+	//		- The breathable pins are 4, 5, 6, 7, 12, 13, 14, 15 only. If tRise and
+	//			tFall are set on 0-3 or 8-11 those pins will still only blink.
+	// 		- ledDriverInit should be called on the pin to be blinked before this.
+	// -----------------------------------------------------------------------------
+	void setupBlink(uint8_t pin, uint8_t tOn, uint8_t toff, uint8_t onIntensity = 255, uint8_t offIntensity = 0,
+					uint8_t tRise = 0, uint8_t tFall = 0, bool log = false, bool openDrain = false);
+
+	// -----------------------------------------------------------------------------
+	// blink(uint8_t pin, unsigned long tOn, unsigned long tOff, uint8_t onIntensity, uint8_t offIntensity);
+	//  	Set a pin to blink output for estimated on/off millisecond durations.
+	//
+	// 	Inputs:
+	//  	- pin: the SX1509 pin (0-15) you want to set blinking
+	//   	- tOn: estimated number of milliseconds the pin is LOW (LED sinking current will be on)
+	//   	- tOff: estimated number of milliseconds the pin is HIGH (LED sinking current will be off)
+	//   	- onIntensity: 0-255 value determining LED on brightness
+	//   	- offIntensity: 0-255 value determining LED off brightness
+	// 	 Notes:
+	//		- The breathable pins are 4, 5, 6, 7, 12, 13, 14, 15 only. If tRise and
+	//			tFall are set on 0-3 or 8-11 those pins will still only blink.
+	// 		- ledDriverInit should be called on the pin to be blinked before this.
+	// -----------------------------------------------------------------------------
+	void blink(uint8_t pin, unsigned long tOn, unsigned long tOff, uint8_t onIntensity = 255, uint8_t offIntensity = 0);
+	
+	// -----------------------------------------------------------------------------
+	// breathe(uint8_t pin, unsigned long tOn, unsigned long tOff, unsigned long rise, unsigned long fall, uint8_t onInt, uint8_t offInt, bool log);
+	//  	Set a pin to breathe output for estimated on/off millisecond durations, with
+	//  	estimated rise and fall durations.
+	//
+	// 	Inputs:
+	//  	- pin: the SX1509 pin (0-15) you want to set blinking
+	//   	- tOn: estimated number of milliseconds the pin is LOW (LED sinking current will be on)
+	//   	- tOff: estimated number of milliseconds the pin is HIGH (LED sinking current will be off)
+	//   	- rise: estimated number of milliseconds the pin rises from LOW to HIGH
+	//   	- falll: estimated number of milliseconds the pin falls from HIGH to LOW
+	//   	- onIntensity: 0-255 value determining LED on brightness
+	//   	- offIntensity: 0-255 value determining LED off brightness
+	// 	 Notes:
+	//		- The breathable pins are 4, 5, 6, 7, 12, 13, 14, 15 only. If tRise and
+	//			tFall are set on 0-3 or 8-11 those pins will still only blink.
+	// 		- ledDriverInit should be called on the pin to be blinked before this,
+	//  	  Or call pinMode(<pin>, ANALOG_OUTPUT);
+	// -----------------------------------------------------------------------------
+	void breathe(uint8_t pin, unsigned long tOn, unsigned long tOff, unsigned long rise, unsigned long fall,
+			uint8_t onInt = 255, uint8_t offInt = 0, bool log = false, bool openDrain = false);
+
+	// -----------------------------------------------------------------------------
+	// keypad(uint8_t rows, uint8_t columns, uint8_t sleepTime, uint8_t scanTime, uint8_t debounceTime)
+	//		Initializes the keypad function on the SX1509. Millisecond durations for sleep,
+	//		scan, and debounce can be set.
+	//
+	//	Inputs:
+	//		- rows: The number of rows in the button matrix.
+	//			- This value must be between 1 and 7. 0 will turn it off.
+	//			- eg: 1 = 2 rows, 2 = 3 rows, 7 = 8 rows, etc.
+	//		- columns: The number of columns in the button matrix
+	//			- This value should be between 0 and 7.
+	//			- 0 = 1 column, 7 = 8 columns, etc.
+	//		- sleepTime: Sets the auto-sleep time of the keypad engine.
+	//  	  Should be a millisecond duration between 0 (OFF) and 8000 (8 seconds).
+	//   	  Possible values are 0, 128, 256, 512, 1000, 2000, 4000, 8000
+	//		- scanTime: Sets the scan time per row. Must be set above debounce.
+	//  	  Should be a millisecond duration between 1 and 128.
+	//   	  Possible values are 1, 2, 4, 8, 16, 32, 64, 128.
+	//		- debounceTime: Sets the debounc time per button. Must be set below scan.
+	//  	  Should be a millisecond duration between 0 and 64.
+	//   	  Possible values are 0 (0.5), 1, 2, 4, 8, 16, 32, 64.
+	// -----------------------------------------------------------------------------
+	void keypad(uint8_t rows, uint8_t columns, unsigned int sleepTime = 0, uint8_t scanTime = 1, uint8_t debounceTime = 0);
+
+	// -----------------------------------------------------------------------------
+	// readKeypad(): This function returns a 16-bit value containing the status of
+	//		keypad engine.
+	//
+	//	Output:
+	//		A 16-bit value is returned. The lower 8 bits represent the up-to 8 rows,
+	//		while the MSB represents the up-to 8 columns. Bit-values of 1 indicate a
+	//		button in that row or column is being pressed. As such, at least two
+	//		bits should be set.
+	// -----------------------------------------------------------------------------
+	uint16_t readKeypad();
+	
+	// -----------------------------------------------------------------------------
+	// getRow(): This function returns the first active row from the return value of
+	//  	readKeypad().
+	//
+	//	Input:
+	//      - keyData: Should be the unsigned int value returned from readKeypad().
+	//	Output:
+	//		A 16-bit value is returned. The lower 8 bits represent the up-to 8 rows,
+	//		while the MSB represents the up-to 8 columns. Bit-values of 1 indicate a
+	//		button in that row or column is being pressed. As such, at least two
+	//		bits should be set.
+	// -----------------------------------------------------------------------------
+	uint8_t getRow(uint16_t keyData);
+	
+	// -----------------------------------------------------------------------------
+	// getCol(): This function returns the first active column from the return value of
+	//  	readKeypad().
+	//
+	//	Input:
+	//      - keyData: Should be the unsigned int value returned from readKeypad().
+	//	Output:
+	//		A 16-bit value is returned. The lower 8 bits represent the up-to 8 rows,
+	//		while the MSB represents the up-to 8 columns. Bit-values of 1 indicate a
+	//		button in that row or column is being pressed. As such, at least two
+	//		bits should be set.
+	// -----------------------------------------------------------------------------
+	uint8_t getCol(uint16_t keyData);
+	
+	// -----------------------------------------------------------------------------
+	// debounceConfig(uint8_t configValue): This method configures the debounce time of
+	//		every input.
+	//
+	//	Input:
+	//		- configValue: A 3-bit value configuring the debounce time.
+	//			000: 0.5ms * 2MHz/fOSC
+	//			001: 1ms * 2MHz/fOSC
+	//			010: 2ms * 2MHz/fOSC
+	//			011: 4ms * 2MHz/fOSC
+	//			100: 8ms * 2MHz/fOSC
+	//			101: 16ms * 2MHz/fOSC
+	//			110: 32ms * 2MHz/fOSC
+	//			111: 64ms * 2MHz/fOSC
+	// -----------------------------------------------------------------------------
+	void debounceConfig(uint8_t configVaule);
+	
+	// -----------------------------------------------------------------------------
+	// debounceTime(uint8_t configValue): This method configures the debounce time of
+	//		every input to an estimated millisecond time duration.
+	//
+	//	Input:
+	//		- time: A millisecond duration estimating the debounce time. Actual
+	//		  debounce time will depend on fOSC. Assuming it's 2MHz, debounce will
+	//		  be set to the 0.5, 1, 2, 4, 8, 16, 32, or 64 ms (whatever's closest)
+	// -----------------------------------------------------------------------------
+	void debounceTime(uint8_t time);
+
+	// -----------------------------------------------------------------------------
+	// debouncePin(uint8_t pin): This method enables debounce on SX1509 input pin.
+	//
+	//	Input:
+	//		- pin: The SX1509 pin to be debounced. Should be between 0 and 15.
+	// -----------------------------------------------------------------------------
+	void debouncePin(uint8_t pin);
+
+	// -----------------------------------------------------------------------------
+	// debounceKeypad(uint8_t pin): This method enables debounce on all pins connected
+	//  to a row/column keypad matrix.
+	//
+	//	Input:
+	//		- time: Millisecond time estimate for debounce (see debounceTime()).
+	//		- numRows: The number of rows in the keypad matrix.
+	//		- numCols: The number of columns in the keypad matrix.
+	// -----------------------------------------------------------------------------
+	void debounceKeypad(uint8_t time, uint8_t numRows, uint8_t numCols);
+
+	// -----------------------------------------------------------------------------
+	// enableInterrupt(uint8_t pin, uint8_t riseFall): This function sets up an interrupt
+	//		on a pin. Interrupts can occur on all SX1509 pins, and can be generated
+	//		on rising, falling, or both.
+	//
+	//	Inputs:
+	//		-pin: SX1509 input pin that will generate an interrupt. Should be 0-15.
+	//		-riseFall: Configures if you want an interrupt generated on rise fall or
+	//			both. For this param, send the pin-change values previously defined
+	//			by Arduino:
+	//			#define CHANGE 1	<- Both
+	//			#define FALLING 2	<- Falling
+	//			#define RISING 3	<- Rising
+	//
+	//	Note: This function does not set up a pin as an input, or configure	its
+	//		pull-up/down resistors! Do that before (or after).
+	// -----------------------------------------------------------------------------
+	void enableInterrupt(uint8_t pin, uint8_t riseFall);
+	
+	// -----------------------------------------------------------------------------
+	// enableInterruptMultiple(uint16_t pins, uint8_t riseFall): This function sets up an interrupt
+	//		on a pin. Interrupts can occur on all SX1509 pins, and can be generated
+	//		on rising, falling, or both.
+	//
+	//	Inputs:
+	//		-pins: SX1509 input pins that will generate an interrupt.
+	//		-riseFall: Configures if you want an interrupt generated on rise fall or
+	//			both. For this param, send the pin-change values previously defined
+	//			by Arduino:
+	//			#define CHANGE 1	<- Both
+	//			#define FALLING 2	<- Falling
+	//			#define RISING 3	<- Rising
+	//
+	//	Note: This function does not set up a pin as an input, or configure	its
+	//		pull-up/down resistors! Do that before (or after).
+	// -----------------------------------------------------------------------------
+	void enableInterruptMultiple(uint16_t pins, uint8_t riseFall);
+
+	// -----------------------------------------------------------------------------
+	// interruptSource(void): Returns an unsigned int representing which pin caused
+	//		an interrupt.
+	//
+	//	Output: 16-bit value, with a single bit set representing the pin(s) that
+	//		generated an interrupt. E.g. a return value of	0x0104 would mean pins 8
+	//		and 3 (bits 8 and 3) have generated an interrupt.
+	//  Input:
+	//  	- clear: boolean commanding whether the interrupt should be cleared
+	//  	  after reading or not.
+	// -----------------------------------------------------------------------------
+	uint16_t interruptSource(bool clear);
+
+	// -----------------------------------------------------------------------------
+	// checkInterrupt(void): Checks if a single pin generated an interrupt.
+	//
+	//	Output: Boolean value. True if the requested pin has triggered an interrupt/
+	//  Input:
+	//  	- pin: Pin to be checked for generating an input.
+	// -----------------------------------------------------------------------------
+	bool checkInterrupt(uint8_t pin);
+
+	// -----------------------------------------------------------------------------
+	// clock(uint8_t oscDivider)
+	//		This function configures the speed of the cloak, which is used to drive LEDs and time debounces.
+	//
+	//	Inputs:
+	//	- oscDivider: Sets the clock divider in REG_MISC. Clock is 2MHz / (1 << (oscDivider - 1). PWM frequency is 1/256 of that.
+	// -----------------------------------------------------------------------------
+	void clock(uint8_t oscDivider);
+};
+
+#endif	// SX1509_H
diff --git a/src/DuetNG/SX1509B.cpp b/src/DuetNG/SX1509B.cpp
deleted file mode 100644
index 7f171f12..00000000
--- a/src/DuetNG/SX1509B.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * SX1509B.cpp
- *
- *  Created on: 19 Oct 2016
- *      Author: David
- */
-
-#include "SX1509B.h"
-
-const uint8_t DueXnAddress = 0x3E;				// address of the SX1509B on the DueX0/DueX2/DueX5
-
-// Initialise the device and identify which expansion board (if any) is attached
-ExpansionBoardType SX1509B::Init()
-{
-	Wire.begin();								// initialise TWI as master
-
-	Wire.beginTransmission(DueXnAddress);		// start a block
-
-	// Return codes from Wire.endTransmission are:
-	// 0: success
-	// 1: data too long to fit in transmit buffer
-	// 2: received NACK on transmit of address
-	// 3: received NACK on transmit of data
-	// 4: other error
-	// We assume that any return code other than 0 means the device was not found.
-	if (Wire.endTransmission() != 0)
-	{
-		// No device found at that address, or a serious error
-		return ExpansionBoardType::none;
-	}
-
-	return ExpansionBoardType::none;	//TODO
-}
-
-
-// End
diff --git a/src/DuetNG/SX1509B.h b/src/DuetNG/SX1509B.h
deleted file mode 100644
index 2fd141ed..00000000
--- a/src/DuetNG/SX1509B.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * SX1509B.h
- *
- *  Created on: 19 Oct 2016
- *      Author: David
- */
-
-#ifndef SRC_DUETNG_SX1509B_H_
-#define SRC_DUETNG_SX1509B_H_
-
-#include "Wire.h"
-
-enum class ExpansionBoardType : uint8_t
-{
-	none,
-	DueX0,
-	DueX2,
-	DueX5
-};
-
-class SX1509B
-{
-public:
-	ExpansionBoardType Init();			// Initialise the device and identify which expansion board (if any) is attached
-};
-
-#endif /* SRC_DUETNG_SX1509B_H_ */
diff --git a/src/DuetNG/SX1509Registers.h b/src/DuetNG/SX1509Registers.h
new file mode 100644
index 00000000..8f8c89cd
--- /dev/null
+++ b/src/DuetNG/SX1509Registers.h
@@ -0,0 +1,165 @@
+/******************************************************************************
+SX1509Registers.h
+Register definitions for SX1509.
+Jim Lindblom @ SparkFun Electronics
+Original Creation Date: September 21, 2015
+https://github.com/sparkfun/SparkFun_SX1509_Arduino_Library
+
+Here you'll find the Arduino code used to interface with the SX1509 I2C
+16 I/O expander. There are functions to take advantage of everything the
+SX1509 provides - input/output setting, writing pins high/low, reading 
+the input value of pins, LED driver utilities (blink, breath, pwm), and
+keypad engine utilites.
+
+Development environment specifics:
+	IDE: Arduino 1.6.5
+	Hardware Platform: Arduino Uno
+	SX1509 Breakout Version: v2.0
+
+This code is beerware; if you see me (or any other SparkFun employee) at the
+local, and you've found our code helpful, please buy us a round!
+
+Distributed as-is; no warranty is given.
+******************************************************************************/
+
+const uint8_t 	REG_INPUT_DISABLE_B		= 0x00;	//	RegInputDisableB Input buffer disable register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_INPUT_DISABLE_A		= 0x01;	//	RegInputDisableA Input buffer disable register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_LONG_SLEW_B			= 0x02;	//	RegLongSlewB Output buffer long slew register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_LONG_SLEW_A			= 0x03;	//	RegLongSlewA Output buffer long slew register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_LOW_DRIVE_B			= 0x04;	//	RegLowDriveB Output buffer low drive register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_LOW_DRIVE_A			= 0x05;	//	RegLowDriveA Output buffer low drive register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_PULL_UP_B			= 0x06;	//	RegPullUpB Pull_up register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_PULL_UP_A			= 0x07;	//	RegPullUpA Pull_up register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_PULL_DOWN_B			= 0x08;	//	RegPullDownB Pull_down register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_PULL_DOWN_A			= 0x09;	//	RegPullDownA Pull_down register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_OPEN_DRAIN_B		= 0x0A;	//	RegOpenDrainB Open drain register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_OPEN_DRAIN_A		= 0x0B;	//	RegOpenDrainA Open drain register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_POLARITY_B			= 0x0C;	//	RegPolarityB Polarity register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_POLARITY_A			= 0x0D;	//	RegPolarityA Polarity register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_DIR_B				= 0x0E;	//	RegDirB Direction register _ I/O[15_8] (Bank B) 1111 1111
+const uint8_t 	REG_DIR_A				= 0x0F;	//	RegDirA Direction register _ I/O[7_0] (Bank A) 1111 1111
+const uint8_t 	REG_DATA_B				= 0x10;	//	RegDataB Data register _ I/O[15_8] (Bank B) 1111 1111*
+const uint8_t 	REG_DATA_A				= 0x11;	//	RegDataA Data register _ I/O[7_0] (Bank A) 1111 1111*
+const uint8_t 	REG_INTERRUPT_MASK_B	= 0x12;	//	RegInterruptMaskB Interrupt mask register _ I/O[15_8] (Bank B) 1111 1111
+const uint8_t 	REG_INTERRUPT_MASK_A	= 0x13;	//	RegInterruptMaskA Interrupt mask register _ I/O[7_0] (Bank A) 1111 1111
+const uint8_t 	REG_SENSE_HIGH_B		= 0x14;	//	RegSenseHighB Sense register for I/O[15:12] 0000 0000
+const uint8_t 	REG_SENSE_LOW_B			= 0x15;	//	RegSenseLowB Sense register for I/O[11:8] 0000 0000
+const uint8_t 	REG_SENSE_HIGH_A		= 0x16;	//	RegSenseHighA Sense register for I/O[7:4] 0000 0000
+const uint8_t 	REG_SENSE_LOW_A			= 0x17;	//	RegSenseLowA Sense register for I/O[3:0] 0000 0000
+const uint8_t 	REG_INTERRUPT_SOURCE_B	= 0x18;	//	RegInterruptSourceB Interrupt source register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_INTERRUPT_SOURCE_A	= 0x19;	//	RegInterruptSourceA Interrupt source register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_EVENT_STATUS_B		= 0x1A;	//	RegEventStatusB Event status register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_EVENT_STATUS_A		= 0x1B;	//	RegEventStatusA Event status register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_LEVEL_SHIFTER_1		= 0x1C;	//	RegLevelShifter1 Level shifter register 0000 0000
+const uint8_t 	REG_LEVEL_SHIFTER_2		= 0x1D;	//	RegLevelShifter2 Level shifter register 0000 0000
+const uint8_t 	REG_CLOCK				= 0x1E;	//	RegClock Clock management register 0000 0000
+const uint8_t 	REG_MISC				= 0x1F;	//	RegMisc Miscellaneous device settings register 0000 0000
+const uint8_t 	REG_LED_DRIVER_ENABLE_B	= 0x20;	//	RegLEDDriverEnableB LED driver enable register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_LED_DRIVER_ENABLE_A	= 0x21;	//	RegLEDDriverEnableA LED driver enable register _ I/O[7_0] (Bank A) 0000 0000
+// Debounce and Keypad Engine		
+const uint8_t 	REG_DEBOUNCE_CONFIG		= 0x22;	//	RegDebounceConfig Debounce configuration register 0000 0000
+const uint8_t 	REG_DEBOUNCE_ENABLE_B	= 0x23;	//	RegDebounceEnableB Debounce enable register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_DEBOUNCE_ENABLE_A	= 0x24;	//	RegDebounceEnableA Debounce enable register _ I/O[7_0] (Bank A) 0000 0000
+const uint8_t 	REG_KEY_CONFIG_1		= 0x25;	//	RegKeyConfig1 Key scan configuration register 0000 0000
+const uint8_t 	REG_KEY_CONFIG_2		= 0x26;	//	RegKeyConfig2 Key scan configuration register 0000 0000
+const uint8_t 	REG_KEY_DATA_1			= 0x27;	//	RegKeyData1 Key value (column) 1111 1111
+const uint8_t 	REG_KEY_DATA_2			= 0x28;	//	RegKeyData2 Key value (row) 1111 1111
+// LED Driver (PWM, blinking, breathing)		
+const uint8_t 	REG_T_ON_0				= 0x29;	//	RegTOn0 ON time register for I/O[0] 0000 0000
+const uint8_t 	REG_I_ON_0				= 0x2A;	//	RegIOn0 ON intensity register for I/O[0] 1111 1111
+const uint8_t 	REG_OFF_0				= 0x2B;	//	RegOff0 OFF time/intensity register for I/O[0] 0000 0000
+const uint8_t 	REG_T_ON_1				= 0x2C;	//	RegTOn1 ON time register for I/O[1] 0000 0000
+const uint8_t 	REG_I_ON_1				= 0x2D;	//	RegIOn1 ON intensity register for I/O[1] 1111 1111
+const uint8_t 	REG_OFF_1				= 0x2E;	//	RegOff1 OFF time/intensity register for I/O[1] 0000 0000
+const uint8_t 	REG_T_ON_2				= 0x2F;	//	RegTOn2 ON time register for I/O[2] 0000 0000
+const uint8_t 	REG_I_ON_2				= 0x30;	//	RegIOn2 ON intensity register for I/O[2] 1111 1111
+const uint8_t 	REG_OFF_2				= 0x31;	//	RegOff2 OFF time/intensity register for I/O[2] 0000 0000
+const uint8_t 	REG_T_ON_3				= 0x32;	//	RegTOn3 ON time register for I/O[3] 0000 0000
+const uint8_t 	REG_I_ON_3				= 0x33;	//	RegIOn3 ON intensity register for I/O[3] 1111 1111
+const uint8_t 	REG_OFF_3				= 0x34;	//	RegOff3 OFF time/intensity register for I/O[3] 0000 0000
+const uint8_t 	REG_T_ON_4				= 0x35;	//	RegTOn4 ON time register for I/O[4] 0000 0000
+const uint8_t 	REG_I_ON_4				= 0x36;	//	RegIOn4 ON intensity register for I/O[4] 1111 1111
+const uint8_t 	REG_OFF_4				= 0x37;	//	RegOff4 OFF time/intensity register for I/O[4] 0000 0000
+const uint8_t 	REG_T_RISE_4			= 0x38;	//	RegTRise4 Fade in register for I/O[4] 0000 0000
+const uint8_t 	REG_T_FALL_4			= 0x39;	//	RegTFall4 Fade out register for I/O[4] 0000 0000
+const uint8_t 	REG_T_ON_5				= 0x3A;	//	RegTOn5 ON time register for I/O[5] 0000 0000
+const uint8_t 	REG_I_ON_5				= 0x3B;	//	RegIOn5 ON intensity register for I/O[5] 1111 1111
+const uint8_t 	REG_OFF_5				= 0x3C;	//	RegOff5 OFF time/intensity register for I/O[5] 0000 0000
+const uint8_t 	REG_T_RISE_5			= 0x3D;	//	RegTRise5 Fade in register for I/O[5] 0000 0000
+const uint8_t 	REG_T_FALL_5			= 0x3E;	//	RegTFall5 Fade out register for I/O[5] 0000 0000
+const uint8_t 	REG_T_ON_6				= 0x3F;	//	RegTOn6 ON time register for I/O[6] 0000 0000
+const uint8_t 	REG_I_ON_6				= 0x40;	//	RegIOn6 ON intensity register for I/O[6] 1111 1111
+const uint8_t 	REG_OFF_6				= 0x41;	//	RegOff6 OFF time/intensity register for I/O[6] 0000 0000
+const uint8_t 	REG_T_RISE_6			= 0x42;	//	RegTRise6 Fade in register for I/O[6] 0000 0000
+const uint8_t 	REG_T_FALL_6			= 0x43;	//	RegTFall6 Fade out register for I/O[6] 0000 0000
+const uint8_t 	REG_T_ON_7				= 0x44;	//	RegTOn7 ON time register for I/O[7] 0000 0000
+const uint8_t 	REG_I_ON_7				= 0x45;	//	RegIOn7 ON intensity register for I/O[7] 1111 1111
+const uint8_t 	REG_OFF_7				= 0x46;	//	RegOff7 OFF time/intensity register for I/O[7] 0000 0000
+const uint8_t 	REG_T_RISE_7			= 0x47;	//	RegTRise7 Fade in register for I/O[7] 0000 0000
+const uint8_t 	REG_T_FALL_7			= 0x48;	//	RegTFall7 Fade out register for I/O[7] 0000 0000
+const uint8_t 	REG_T_ON_8				= 0x49;	//	RegTOn8 ON time register for I/O[8] 0000 0000
+const uint8_t 	REG_I_ON_8				= 0x4A;	//	RegIOn8 ON intensity register for I/O[8] 1111 1111
+const uint8_t 	REG_OFF_8				= 0x4B;	//	RegOff8 OFF time/intensity register for I/O[8] 0000 0000
+const uint8_t 	REG_T_ON_9				= 0x4C;	//	RegTOn9 ON time register for I/O[9] 0000 0000
+const uint8_t 	REG_I_ON_9				= 0x4D;	//	RegIOn9 ON intensity register for I/O[9] 1111 1111
+const uint8_t 	REG_OFF_9				= 0x4E;	//	RegOff9 OFF time/intensity register for I/O[9] 0000 0000
+const uint8_t 	REG_T_ON_10				= 0x4F;	//	RegTOn10 ON time register for I/O[10] 0000 0000
+const uint8_t 	REG_I_ON_10				= 0x50;	//	RegIOn10 ON intensity register for I/O[10] 1111 1111
+const uint8_t 	REG_OFF_10				= 0x51;	//	RegOff10 OFF time/intensity register for I/O[10] 0000 0000
+const uint8_t 	REG_T_ON_11				= 0x52;	//	RegTOn11 ON time register for I/O[11] 0000 0000
+const uint8_t 	REG_I_ON_11				= 0x53;	//	RegIOn11 ON intensity register for I/O[11] 1111 1111
+const uint8_t 	REG_OFF_11				= 0x54;	//	RegOff11 OFF time/intensity register for I/O[11] 0000 0000
+const uint8_t 	REG_T_ON_12				= 0x55;	//	RegTOn12 ON time register for I/O[12] 0000 0000
+const uint8_t 	REG_I_ON_12				= 0x56;	//	RegIOn12 ON intensity register for I/O[12] 1111 1111
+const uint8_t 	REG_OFF_12				= 0x57;	//	RegOff12 OFF time/intensity register for I/O[12] 0000 0000
+const uint8_t 	REG_T_RISE_12			= 0x58;	//	RegTRise12 Fade in register for I/O[12] 0000 0000
+const uint8_t 	REG_T_FALL_12			= 0x59;	//	RegTFall12 Fade out register for I/O[12] 0000 0000
+const uint8_t 	REG_T_ON_13				= 0x5A;	//	RegTOn13 ON time register for I/O[13] 0000 0000
+const uint8_t 	REG_I_ON_13				= 0x5B;	//	RegIOn13 ON intensity register for I/O[13] 1111 1111
+const uint8_t 	REG_OFF_13				= 0x5C;	//	RegOff13 OFF time/intensity register for I/O[13] 0000 0000
+const uint8_t 	REG_T_RISE_13			= 0x5D;	//	RegTRise13 Fade in register for I/O[13] 0000 0000
+const uint8_t 	REG_T_FALL_13			= 0x5E;	//	RegTFall13 Fade out register for I/O[13] 0000 0000
+const uint8_t 	REG_T_ON_14				= 0x5F;	//	RegTOn14 ON time register for I/O[14] 0000 0000
+const uint8_t 	REG_I_ON_14				= 0x60;	//	RegIOn14 ON intensity register for I/O[14] 1111 1111
+const uint8_t 	REG_OFF_14				= 0x61;	//	RegOff14 OFF time/intensity register for I/O[14] 0000 0000
+const uint8_t 	REG_T_RISE_14			= 0x62;	//	RegTRise14 Fade in register for I/O[14] 0000 0000
+const uint8_t 	REG_T_FALL_14			= 0x63;	//	RegTFall14 Fade out register for I/O[14] 0000 0000
+const uint8_t 	REG_T_ON_15				= 0x64;	//	RegTOn15 ON time register for I/O[15] 0000 0000
+const uint8_t 	REG_I_ON_15				= 0x65;	//	RegIOn15 ON intensity register for I/O[15] 1111 1111
+const uint8_t 	REG_OFF_15				= 0x66;	//	RegOff15 OFF time/intensity register for I/O[15] 0000 0000
+const uint8_t 	REG_T_RISE_15			= 0x67;	//	RegTRise15 Fade in register for I/O[15] 0000 0000
+const uint8_t 	REG_T_FALL_15			= 0x68;	//	RegTFall15 Fade out register for I/O[15] 0000 0000
+// 	Miscellaneous		
+const uint8_t 	REG_HIGH_INPUT_B		= 0x69;	//	RegHighInputB High input enable register _ I/O[15_8] (Bank B) 0000 0000
+const uint8_t 	REG_HIGH_INPUT_A		= 0x6A;	//	RegHighInputA High input enable register _ I/O[7_0] (Bank A) 0000 0000
+//  Software Reset		
+const uint8_t 	REG_RESET				= 0x7D;	//	RegReset Software reset register 0000 0000
+const uint8_t 	REG_TEST_1				= 0x7E;	//	RegTest1 Test register 0000 0000
+const uint8_t 	REG_TEST_2				= 0x7F;	//	RegTest2 Test register 0000 0000
+
+const uint8_t REG_I_ON[16] = {REG_I_ON_0, REG_I_ON_1, REG_I_ON_2, REG_I_ON_3,
+					REG_I_ON_4, REG_I_ON_5, REG_I_ON_6, REG_I_ON_7,
+					REG_I_ON_8, REG_I_ON_9, REG_I_ON_10, REG_I_ON_11,
+					REG_I_ON_12, REG_I_ON_13, REG_I_ON_14, REG_I_ON_15};
+					
+const uint8_t REG_T_ON[16] = {REG_T_ON_0, REG_T_ON_1, REG_T_ON_2, REG_T_ON_3,
+					REG_T_ON_4, REG_T_ON_5, REG_T_ON_6, REG_T_ON_7,
+					REG_T_ON_8, REG_T_ON_9, REG_T_ON_10, REG_T_ON_11,
+					REG_T_ON_12, REG_T_ON_13, REG_T_ON_14, REG_T_ON_15};
+					
+const uint8_t REG_OFF[16] = {REG_OFF_0, REG_OFF_1, REG_OFF_2, REG_OFF_3,
+					REG_OFF_4, REG_OFF_5, REG_OFF_6, REG_OFF_7,
+					REG_OFF_8, REG_OFF_9, REG_OFF_10, REG_OFF_11,
+					REG_OFF_12, REG_OFF_13, REG_OFF_14, REG_OFF_15};
+
+const uint8_t REG_T_RISE[16] = {0xFF, 0xFF, 0xFF, 0xFF,
+					REG_T_RISE_4, REG_T_RISE_5, REG_T_RISE_6, REG_T_RISE_7,
+					0xFF, 0xFF, 0xFF, 0xFF,
+					REG_T_RISE_12, REG_T_RISE_13, REG_T_RISE_14, REG_T_RISE_15};
+					
+const uint8_t REG_T_FALL[16] = {0xFF, 0xFF, 0xFF, 0xFF,
+					REG_T_FALL_4, REG_T_FALL_5, REG_T_FALL_6, REG_T_FALL_7,
+					0xFF, 0xFF, 0xFF, 0xFF,
+					REG_T_FALL_12, REG_T_FALL_13, REG_T_FALL_14, REG_T_FALL_15};
+
+// End
diff --git a/src/DuetNG/Webserver.cpp b/src/DuetNG/Webserver.cpp
index 790b23ec..7226df68 100644
--- a/src/DuetNG/Webserver.cpp
+++ b/src/DuetNG/Webserver.cpp
@@ -513,24 +513,30 @@ void Webserver::HandleGCodeReply(const WebSource source, const char *reply)
 }
 
 //----------------------------------------------------------------------------------------------------
+// Return the value of the specified key, or nullptr if not present
+const char* Webserver::GetKeyValue(const char *key) const
+{
+	for (size_t i = 0; i < numQualKeys; ++i)
+	{
+		if (StringEquals(qualifiers[i].key, key))
+		{
+			return qualifiers[i].value;
+		}
+	}
+	return nullptr;
+}
 
 // Process the first fragment of input from the client.
 // Return true if the session should be kept open.
 bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command, bool isOnlyFragment)
 {
-	// Get the first two key/value pairs
-	const char* key1 = (numQualKeys >= 1) ? qualifiers[0].key : "";
-	const char* value1 = (numQualKeys >= 1) ? qualifiers[0].value : "";
-	const char* key2 = (numQualKeys >= 2) ? qualifiers[1].key : "";
-	const char* value2 = (numQualKeys >= 2) ? qualifiers[1].value : "";
-
 	// Process connect messages first
-	if (StringEquals(command, "connect") && StringEquals(key1, "password"))
+	if (StringEquals(command, "connect") && GetKeyValue("password") != nullptr)
 	{
 		OutputBuffer *response;
 		if (OutputBuffer::Allocate(response))
 		{
-			if (session.isAuthenticated || reprap.CheckPassword(value1))
+			if (session.isAuthenticated || reprap.CheckPassword(GetKeyValue("password")))
 			{
 				// Password OK
 				session.isAuthenticated = true;
@@ -588,19 +594,21 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 		return false;
 	}
 
-	if (StringEquals(command, "download") && StringEquals(key1, "name"))
+	if (StringEquals(command, "download") && GetKeyValue("name") != nullptr)
 	{
-		SendFile(value1, session);
+		SendFile(GetKeyValue("name"), session);
 		return false;
 	}
 
 	if (StringEquals(command, "upload"))
 	{
-		if (StringEquals(key1, "name") && StringEquals(key2, "length"))
+		const char* nameVal = GetKeyValue("name");
+		const char* lengthVal = GetKeyValue("length");
+		if (nameVal != nullptr && lengthVal != nullptr)
 		{
 			// Deal with file upload request
-			uint32_t fileLength = atol(value2);
-			StartUpload(session, value1, fileLength);
+			uint32_t fileLength = atol(lengthVal);
+			StartUpload(session, nameVal, fileLength);
 			if (session.uploadState == uploading)
 			{
 				if (isOnlyFragment)
@@ -623,9 +631,11 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 	if (StringEquals(command, "move"))
 	{
 		const char* response =  "{\"err\":1}";		// assume failure
-		if (StringEquals(key1, "old") && StringEquals(key2, "new"))
+		const char* oldVal = GetKeyValue("old");
+		const char* newVal = GetKeyValue("new");
+		if (oldVal != nullptr && newVal != nullptr)
 		{
-			bool success = platform->GetMassStorage()->Rename(value1, value2);
+			bool success = platform->GetMassStorage()->Rename(oldVal, newVal);
 			if (success)
 			{
 				response =  "{\"err\":0}";
@@ -638,9 +648,10 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 	if (StringEquals(command, "mkdir"))
 	{
 		const char* response =  "{\"err\":1}";		// assume failure
-		if (StringEquals(key1, "dir"))
+		const char* dirVal = GetKeyValue("dir");
+		if (dirVal != nullptr)
 		{
-			bool ok = (platform->GetMassStorage()->MakeDirectory(value1));
+			bool ok = (platform->GetMassStorage()->MakeDirectory(dirVal));
 			if (ok)
 			{
 				response =  "{\"err\":0}";
@@ -653,9 +664,10 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 	if (StringEquals(command, "delete"))
 	{
 		const char* response =  "{\"err\":1}";		// assume failure
-		if (StringEquals(key1, "name"))
+		const char* nameVal = GetKeyValue("name");
+		if (nameVal != nullptr)
 		{
-			bool ok = platform->GetMassStorage()->Delete("0:/", value1);
+			bool ok = platform->GetMassStorage()->Delete("0:/", nameVal);
 			if (ok)
 			{
 				response =  "{\"err\":0}";
@@ -669,11 +681,11 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 	OutputBuffer *response = nullptr;
 	if (StringEquals(command, "status"))
 	{
-		int type = 0;
-		if (StringEquals(key1, "type"))
+		const char* typeVal = GetKeyValue("type");
+		if (typeVal != nullptr)
 		{
 			// New-style JSON status responses
-			type = atoi(value1);
+			int type = atoi(typeVal);
 			if (type < 1 || type > 3)
 			{
 				type = 1;
@@ -688,9 +700,10 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 	}
 	else if (StringEquals(command, "gcode"))
 	{
-		if (StringEquals(key1, "gcode"))
+		const char* gcodeVal = GetKeyValue("gcode");
+		if (gcodeVal != nullptr)
 		{
-			LoadGcodeBuffer(value1);
+			LoadGcodeBuffer(gcodeVal);
 			if (OutputBuffer::Allocate(response))
 			{
 				response->printf("{\"buff\":%u}", GetGCodeBufferSpace());
@@ -702,26 +715,25 @@ bool Webserver::ProcessFirstFragment(HttpSession& session, const char* command,
 			return false;
 		}
 	}
-	else if (StringEquals(command, "filelist") && StringEquals(key1, "dir"))
+	else if (StringEquals(command, "filelist") && GetKeyValue("dir") != nullptr)
 	{
-		response = reprap.GetFilelistResponse(value1);
+		response = reprap.GetFilelistResponse(GetKeyValue("dir"));
 	}
 	else if (StringEquals(command, "files"))
 	{
-		const char* dir = (StringEquals(key1, "dir")) ? value1 : platform->GetGCodeDir();
-		bool flagDirs = false;
-		if (numQualKeys >= 2)
+		const char* dir = GetKeyValue("dir");
+		if (dir == nullptr)
 		{
-			if (StringEquals(qualifiers[1].key, "flagDirs"))
-			{
-				flagDirs = StringEquals(qualifiers[1].value, "1");
-			}
+			dir = platform->GetGCodeDir();
 		}
+		const char* flagDirsVal = GetKeyValue("flagDirs");
+		bool flagDirs = flagDirsVal != nullptr && atoi(flagDirsVal) == 1;
 		response = reprap.GetFilesResponse(dir, flagDirs);
 	}
 	else if (StringEquals(command, "fileinfo"))
 	{
-		if (reprap.GetPrintMonitor()->GetFileInfoResponse(StringEquals(key1, "name") ? value1 : nullptr, response))
+		const char* nameVal = GetKeyValue("name");
+		if (reprap.GetPrintMonitor()->GetFileInfoResponse(nameVal, response))
 		{
 			processingDeferredRequest = false;
 		}
diff --git a/src/DuetNG/Webserver.h b/src/DuetNG/Webserver.h
index 3b7a0a71..8b361f60 100644
--- a/src/DuetNG/Webserver.h
+++ b/src/DuetNG/Webserver.h
@@ -122,6 +122,8 @@ private:
 	HttpSession *FindSession(uint32_t ip);			// find an existing session for this requester
 	void DeleteSession(uint32_t ip);				// delete a session
 
+	const char* GetKeyValue(const char *key) const;	// return the value of the specified key, or nullptr if not present
+
 	// Response from GCodes class
 	OutputStack *gcodeReply;
 
diff --git a/src/Fan.cpp b/src/Fan.cpp
index 1e638632..b4358238 100644
--- a/src/Fan.cpp
+++ b/src/Fan.cpp
@@ -61,14 +61,14 @@ void Fan::SetInverted(bool inv)
 
 void Fan::SetHardwarePwm(float pwmVal)
 {
-	if (pin >= 0)
+	if (pin != NoPin)
 	{
 		bool invert = hardwareInverted;
 		if (inverted)
 		{
 			invert = !invert;
 		}
-		AnalogOut(pin, (invert) ? (1.0 - pwmVal) : pwmVal, freq);
+		Platform::WriteAnalog(pin, (invert) ? (1.0 - pwmVal) : pwmVal, freq);
 	}
 }
 
diff --git a/src/Fan.h b/src/Fan.h
index eb8c15c5..9193428b 100644
--- a/src/Fan.h
+++ b/src/Fan.h
@@ -28,6 +28,7 @@ private:
 	void SetHardwarePwm(float pwmVal);
 
 public:
+	bool IsEnabled() const { return pin != NoPin; }
 	float GetValue() const { return val; }
 	float GetMinValue() const { return minVal; }
 	float GetBlipTime() const { return (float)blipTime * MillisToSeconds; }
@@ -45,6 +46,7 @@ public:
 	void SetTriggerTemperature(float t) { triggerTemperature = t; }
 	void SetHeatersMonitored(uint16_t h);
 	void Check();
+	void Disable() { pin = NoPin; }
 };
 
 #endif /* SRC_FAN_H_ */
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 50e83a9c..a826a91c 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -45,6 +45,9 @@ const char* HOME_DELTA_G = "homedelta.g";
 
 const size_t gcodeReplyLength = 2048;			// long enough to pass back a reasonable number of files in response to M20
 
+const float MinServoPulseWidth = 544.0, MaxServoPulseWidth = 2400.0;
+const uint16_t ServoRefreshFrequency = 50;
+
 void GCodes::RestorePoint::Init()
 {
 	for (size_t i = 0; i < DRIVES; ++i)
@@ -103,7 +106,8 @@ void GCodes::Init()
 		pausedFanValues[i] = 0.0;
 	}
 
-	retractLength = retractExtra = retractSpeed = retractHop = 0.0;
+	retractLength = retractExtra = retractHop = 0.0;
+	retractSpeed = unRetractSpeed = 600.0;
 }
 
 // This is called from Init and when doing an emergency stop
@@ -2511,7 +2515,7 @@ void GCodes::SetToolHeaters(Tool *tool, float temperature)
 // Retract or un-retract filament, returning true if movement has been queued, false if this needs to be called again
 bool GCodes::RetractFilament(bool retract)
 {
-	if (retractLength != 0.0 || retractHop != 0.0 || (retract && retractExtra != 0.0))
+	if (retractLength != 0.0 || retractHop != 0.0 || (!retract && retractExtra != 0.0))
 	{
 		const Tool *tool = reprap.GetCurrentTool();
 		if (tool != nullptr)
@@ -2530,13 +2534,15 @@ bool GCodes::RetractFilament(bool retract)
 					moveBuffer.coords[i] = 0.0;
 				}
 				// Set the feed rate. If there is any Z hop then we need to pass the Z speed, else we pass the extrusion speed.
+				const float speedToUse = (retract) ? retractSpeed : unRetractSpeed;
 				moveBuffer.feedRate = (retractHop == 0.0)
-										? retractSpeed * secondsToMinutes
-										: retractSpeed * secondsToMinutes * retractHop/retractLength;
-				moveBuffer.coords[Z_AXIS] += ((retract) ? retractHop : -retractHop);
+										? speedToUse * secondsToMinutes
+										: speedToUse * secondsToMinutes * retractHop/retractLength;
+				moveBuffer.coords[Z_AXIS] += (retract) ? retractHop : -retractHop;
+				const float lengthToUse = (retract) ? -retractLength : retractLength + retractExtra;
 				for (size_t i = 0; i < nDrives; ++i)
 				{
-					moveBuffer.coords[E0_AXIS + tool->Drive(i)] = (retract) ? -retractLength : retractLength + retractExtra;
+					moveBuffer.coords[E0_AXIS + tool->Drive(i)] = lengthToUse;
 				}
 
 				moveBuffer.isFirmwareRetraction = true;
@@ -3190,16 +3196,31 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 42:	// Turn an output pin on or off
 		if (gb->Seen('P'))
 		{
-			int pin = gb->GetIValue();
-			if (gb->Seen('S'))
+			const int logicalPin = gb->GetIValue();
+			Pin pin;
+			bool invert;
+			if (platform->GetFirmwarePin(logicalPin, PinAccess::write, pin, invert))
 			{
-				int val = gb->GetIValue();
-				bool success = platform->SetPin(pin, val);
-				if (!success)
+				if (gb->Seen('S'))
 				{
-					error = true;
-					reply.printf("Setting pin %d to %d is not supported", pin, val);
+					float val = gb->GetFValue();
+					if (val > 1.0)
+					{
+						val /= 255.0;
+					}
+					val = constrain<float>(val, 0.0, 1.0);
+					if (invert)
+					{
+						val = 1.0 - val;
+					}
+					Platform::WriteAnalog(pin, val, DefaultPinWritePwmFreq);
 				}
+				// Ignore the command if no S parameter provided
+			}
+			else
+			{
+				reply.printf("Logical pin %d is not available for writing", logicalPin);
+				error = true;
 			}
 		}
 		break;
@@ -3392,7 +3413,15 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 				if (gb->Seen('I'))		// Invert cooling
 				{
-					fan.SetInverted(gb->GetIValue() > 0);
+					int invert = gb->GetIValue();
+					if (invert < 0)
+					{
+						fan.Disable();
+					}
+					else
+					{
+						fan.SetInverted(gb->GetIValue() > 0);
+					}
 					seen = true;
 				}
 
@@ -3550,7 +3579,15 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		else
 		{
-			reply.printf("FIRMWARE_NAME: %s FIRMWARE_VERSION: %s ELECTRONICS: %s DATE: %s", NAME, VERSION, platform->GetElectronicsString(), DATE);
+			reply.printf("FIRMWARE_NAME: %s FIRMWARE_VERSION: %s ELECTRONICS: %s", NAME, VERSION, platform->GetElectronicsString());
+#ifdef DUET_NG
+			const char* expansionName = DuetExpansion::GetExpansionBoardName();
+			if (expansionName != nullptr)
+			{
+				reply.catf(" + %s", expansionName);
+			}
+#endif
+			reply.catf(" FIRMWARE_DATE: %s", DATE);
 		}
 		break;
 
@@ -3956,12 +3993,17 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 			if (gb->Seen('R'))
 			{
-				retractExtra = max<float>(gb->GetFValue(), 0.0);
+				retractExtra = max<float>(gb->GetFValue(), -retractLength);
 				seen = true;
 			}
 			if (gb->Seen('F'))
 			{
-				retractSpeed = max<float>(gb->GetFValue(), 60.0);
+				unRetractSpeed = retractSpeed = max<float>(gb->GetFValue(), 60.0);
+				seen = true;
+			}
+			if (gb->Seen('T'))	// must do this one after 'F'
+			{
+				unRetractSpeed = max<float>(gb->GetFValue(), 60.0);
 				seen = true;
 			}
 			if (gb->Seen('Z'))
@@ -3971,8 +4013,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 			if (!seen)
 			{
-				reply.printf("Retraction settings: length %.2fmm, extra length %.2fmm, speed %dmm/min, Z hop %.2fmm",
-						retractLength, retractExtra, (int)retractSpeed, retractHop);
+				reply.printf("Retraction settings: length %.2f/%.2fmm, speed %d/%dmm/min, Z hop %.2fmm",
+						retractLength, retractLength + retractExtra, (int)retractSpeed, (int)unRetractSpeed, retractHop);
 			}
 		}
 		break;
@@ -4072,6 +4114,50 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 		// For case 226, see case 25
 
+	case 280:	// Servos
+		if (gb->Seen('P'))
+		{
+			const int servoIndex = gb->GetIValue();
+			Pin servoPin;
+			bool invert;
+			if (platform->GetFirmwarePin(servoIndex, PinAccess::servo, servoPin, invert))
+			{
+				if (gb->Seen('S'))
+				{
+					float angleOrWidth = gb->GetFValue();
+					if (angleOrWidth < 0.0)
+					{
+						// Disable the servo by setting the pulse width to zero
+						Platform::WriteAnalog(servoPin, 0.0, ServoRefreshFrequency);
+					}
+					else
+					{
+						if (angleOrWidth < MinServoPulseWidth)
+						{
+							// User gave an angle so convert it to a pulse width in microseconds
+							angleOrWidth = (min<float>(angleOrWidth, 180.0) * ((MaxServoPulseWidth - MinServoPulseWidth) / 180.0)) + MinServoPulseWidth;
+						}
+						else if (angleOrWidth > MaxServoPulseWidth)
+						{
+							angleOrWidth = MaxServoPulseWidth;
+						}
+						float pwm = angleOrWidth * (ServoRefreshFrequency/1e6);
+						if (invert)
+						{
+							pwm = 1.0 - pwm;
+						}
+						Platform::WriteAnalog(servoPin, pwm, ServoRefreshFrequency);
+					}
+				}
+				// We don't currently allow the servo position to be read back
+			}
+			else
+			{
+				platform->MessageF(GENERIC_MESSAGE, "Error: Invalid servo index %d in M280 command\n", servoIndex);
+			}
+		}
+		break;
+
 	case 300:	// Beep
 		{
 			int ms = (gb->Seen('P')) ? gb->GetIValue() : 1000;			// time in milliseconds
@@ -4166,6 +4252,10 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 						reply.copy("Error: bad model parameters");
 					}
 				}
+				else if (!model.IsEnabled())
+				{
+					reply.printf("Heater %u is disabled");
+				}
 				else
 				{
 					reply.printf("Heater %u model: gain %.1f, time constant %.1f, dead time %.1f, max PWM %.2f, in use: %s, mode: %s",
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 2cd3de46..af470431 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -292,6 +292,7 @@ private:
     // Firmware retraction settings
     float retractLength, retractExtra;			// retraction length and extra length to un-retract
     float retractSpeed;							// retract speed in mm/min
+    float unRetractSpeed;						// un=retract speed in mm/min
     float retractHop;							// Z hop when retracting
 
     // Triggers
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index 402b8354..f654ec61 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -38,6 +38,13 @@ void Heat::Init()
 		{
 			pids[heater]->Init(DefaultBedHeaterGain, DefaultBedHeaterTimeConstant, DefaultBedHeaterDeadTime, false);
 		}
+#ifndef DUET_NG
+		else if (heater == HEATERS - 1)
+		{
+			// Heater 6 pin is shared with fan 1. By default we support fan 1, so disable heater 6.
+			pids[heater]->Init(-1.0, -1.0, -1.0, true);
+		}
+#endif
 		else
 		{
 			pids[heater]->Init(DefaultHotEndHeaterGain, DefaultHotEndHeaterTimeConstant, DefaultHotEndHeaterDeadTime, true);
diff --git a/src/Heating/Heat.h b/src/Heating/Heat.h
index c3e52bf9..e546ce40 100644
--- a/src/Heating/Heat.h
+++ b/src/Heating/Heat.h
@@ -103,6 +103,9 @@ public:
 	void SetHeaterProtection(size_t heater, float maxTempExcursion, float maxFaultTime)
 	pre(heater < HEATERS);
 
+	bool IsHeaterEnabled(size_t heater) const					// Is this heater enabled?
+	pre(heater < HEATERS);
+
 private:
 	Platform* platform;											// The instance of the RepRap hardware class
 	PID* pids[HEATERS];											// A PID controller for each heater
@@ -179,6 +182,12 @@ inline void Heat::UseModel(size_t heater, bool b)
 	pids[heater]->UseModel(b);
 }
 
+// Is the heater enabled?
+inline bool Heat::IsHeaterEnabled(size_t heater) const
+{
+	return pids[heater]->IsHeaterEnabled();
+}
+
 inline void Heat::GetHeaterProtection(size_t heater, float& maxTempExcursion, float& maxFaultTime) const
 {
 	pids[heater]->GetHeaterProtection(maxTempExcursion, maxFaultTime);
diff --git a/src/Heating/Pid.cpp b/src/Heating/Pid.cpp
index a40520e3..e1a8190c 100644
--- a/src/Heating/Pid.cpp
+++ b/src/Heating/Pid.cpp
@@ -36,9 +36,12 @@ void PID::Init(float pGain, float pTc, float pTd, bool usePid)
 	maxTempExcursion = DefaultMaxTempExcursion;
 	maxHeatingFaultTime = DefaultMaxHeatingFaultTime;
 	model.SetParameters(pGain, pTc, pTd, 1.0, usePid);
-
 	Reset();
-	SetHeater(0.0);
+
+	if (model.IsEnabled())
+	{
+		SetHeater(0.0);
+	}
 
 	// Time the sensor was last sampled.  During startup, we use the current
 	// time as the initial value so as to not trigger an immediate warning from the Tick ISR.
@@ -68,6 +71,13 @@ bool PID::SetModel(float gain, float tc, float td, float maxPwm, bool usePid)
 	const bool rslt = model.SetParameters(gain, tc, td, maxPwm, usePid);
 	if (rslt)
 	{
+#if !defined(DUET_NG) && !defined(__RADDS__)
+		if (heater == HEATERS - 1)
+		{
+			// The last heater on the Duet 0.8.5 + DueX4 shares its pin with Fan1
+			reprap.GetPlatform()->EnableSharedFan(!model.IsEnabled());
+		}
+#endif
 		if (model.IsEnabled())
 		{
 			const float safeGain = (heater == reprap.GetHeat()->GetBedHeater() || heater == reprap.GetHeat()->GetChamberHeater())
@@ -462,7 +472,11 @@ float PID::GetExpectedHeatingRate() const
 void PID::StartAutoTune(float maxTemp, float maxPwm, StringRef& reply)
 {
 	// Starting an auto tune
-	if (lastPwm > 0.0 || GetAveragePWM() > 0.02)
+	if (!model.IsEnabled())
+	{
+		reply.printf("Error: heater %d cannot be auto tuned while it is disabled", heater);
+	}
+	else if (lastPwm > 0.0 || GetAveragePWM() > 0.02)
 	{
 		reply.printf("Error: heater %d must be off and cold before auto tuning it", heater);
 	}
diff --git a/src/Heating/Pid.h b/src/Heating/Pid.h
index 90e98e2b..67e019c4 100644
--- a/src/Heating/Pid.h
+++ b/src/Heating/Pid.h
@@ -68,6 +68,9 @@ public:
 	void UseModel(bool b)							// Use or don't use the model to provide the PID parameters
 		{ useModel = b; }
 
+	bool IsHeaterEnabled() const					// Is this heater enabled?
+		{ return model.IsEnabled(); }
+
 	void GetHeaterProtection(float& pMaxTempExcursion, float& pMaxFaultTime) const
 		{ pMaxTempExcursion = maxTempExcursion; pMaxFaultTime = maxHeatingFaultTime; }
 
diff --git a/src/Platform.cpp b/src/Platform.cpp
index a6c1e64e..43427d75 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -141,8 +141,6 @@ bool PidParameters::operator==(const PidParameters& other) const
 //*************************************************************************************************
 // Platform class
 
-/*static*/ const uint8_t Platform::pinAccessAllowed[NUM_PINS_ALLOWED/8] = PINS_ALLOWED;
-
 Platform::Platform() :
 		autoSaveEnabled(false), board(DEFAULT_BOARD_TYPE), active(false), errorCodeBits(0),
 		auxGCodeReply(nullptr), fileStructureInitialised(false), tickState(0), debugCode(0)
@@ -320,10 +318,6 @@ void Platform::Init()
 		pinMode(STEP_PINS[drive], OUTPUT_LOW);
 		pinMode(DIRECTION_PINS[drive], OUTPUT_LOW);
 		pinMode(ENABLE_PINS[drive], OUTPUT_HIGH);				// this is OK for the TMC2660 CS pins too
-		if (endStopPins[drive] != NoPin)
-		{
-			pinMode(endStopPins[drive], INPUT_PULLUP);			// enable pullup resistor so that expansion connector pins can be used as trigger inputs
-		}
 
 		const PinDescription& pinDesc = g_APinDescription[STEP_PINS[drive]];
 		pinDesc.pPort->PIO_OWER = pinDesc.ulPin;				// enable parallel writes to the step pins
@@ -338,7 +332,9 @@ void Platform::Init()
 
 #ifdef DUET_NG
 	// Test for presence of a DueX2 or DueX5 expansion board and work out how many TMC2660 drivers we have
-	ExpansionBoardType et = expansion.Init();
+	// The SX1509B has an independent power on reset, so give it some time
+	delay(200);
+	ExpansionBoardType et = DuetExpansion::Init();
 	switch (et)
 	{
 	case ExpansionBoardType::DueX2:
@@ -428,7 +424,7 @@ void Platform::Init()
 #endif
 
 	// Clear the spare pin configuration
-	memset(pinInitialised, 0, sizeof(pinInitialised));
+	memset(logicalPinModes, PIN_MODE_NOT_CONFIGURED, sizeof(logicalPinModes));		// set all pins to "not configured"
 
 	// Kick everything off
 	lastTime = Time();
@@ -535,6 +531,12 @@ void Platform::InitZProbe()
 	case 6:
 		AnalogInEnableChannel(zProbeAdcChannel, false);
 		pinMode(zProbePin, INPUT_PULLUP);
+		pinMode(endStopPins[E0_AXIS + 1], INPUT_PULLUP);
+		break;
+
+	case 7:
+		AnalogInEnableChannel(zProbeAdcChannel, false);
+		pinMode(zProbePin, INPUT_PULLUP);
 		break;	//TODO (DeltaProbe)
 	}
 }
@@ -552,6 +554,7 @@ int Platform::ZProbe() const
 		case 3:		// Alternate sensor
 		case 4:		// Switch connected to E0 endstop input
 		case 5:		// Switch connected to Z probe input
+		case 6:		// Switch connected to E1 endstop input
 			zProbeVal = (int) ((zProbeOnFilter.GetSum() + zProbeOffFilter.GetSum()) / (8 * Z_PROBE_AVERAGE_READINGS));
 			break;
 
@@ -561,7 +564,7 @@ int Platform::ZProbe() const
 			zProbeVal = (int) (((int32_t) zProbeOnFilter.GetSum() - (int32_t) zProbeOffFilter.GetSum()) / (int)(4 * Z_PROBE_AVERAGE_READINGS));
 			break;
 
-		case 6:		// Delta humming probe
+		case 7:		// Delta humming probe
 			zProbeVal = (int) ((zProbeOnFilter.GetSum() + zProbeOffFilter.GetSum()) / (8 * Z_PROBE_AVERAGE_READINGS));	//TODO this is temporary
 			break;
 
@@ -637,7 +640,7 @@ float Platform::GetZProbeTravelSpeed() const
 
 void Platform::SetZProbeType(int pt)
 {
-	int newZProbeType = (pt >= 0 && pt <= 6) ? pt : 0;
+	int newZProbeType = (pt >= 0 && pt <= 7) ? pt : 0;
 	if (newZProbeType != nvData.zProbeType)
 	{
 		nvData.zProbeType = newZProbeType;
@@ -657,54 +660,44 @@ const ZProbeParameters& Platform::GetZProbeParameters() const
 	case 2:
 		return nvData.irZProbeParameters;
 	case 3:
-	case 6:
+	case 7:
 		return nvData.alternateZProbeParameters;
 	case 4:
 	case 5:
+	case 6:
 	default:
 		return nvData.switchZProbeParameters;
 	}
 }
 
-bool Platform::SetZProbeParameters(const struct ZProbeParameters& params)
+void Platform::SetZProbeParameters(const struct ZProbeParameters& params)
 {
-	switch (nvData.zProbeType)
+	if (GetZProbeParameters() != params)
 	{
-	case 1:
-	case 2:
-		if (nvData.irZProbeParameters != params)
+		switch (nvData.zProbeType)
 		{
+		case 1:
+		case 2:
 			nvData.irZProbeParameters = params;
-			if (autoSaveEnabled)
-			{
-				WriteNvData();
-			}
-		}
-		return true;
-	case 3:
-	case 6:
-		if (nvData.alternateZProbeParameters != params)
-		{
+			break;
+
+		case 3:
+		case 7:
 			nvData.alternateZProbeParameters = params;
-			if (autoSaveEnabled)
-			{
-				WriteNvData();
-			}
+			break;
+
+		case 4:
+		case 5:
+		case 6:
+		default:
+			nvData.switchZProbeParameters = params;
+			break;
 		}
-		return true;
-	case 4:
-	case 5:
-		if (nvData.switchZProbeParameters != params)
+
+		if (autoSaveEnabled)
 		{
-			nvData.switchZProbeParameters = params;
-			if (autoSaveEnabled)
-			{
-				WriteNvData();
-			}
+			WriteNvData();
 		}
-		return true;
-	default:
-		return false;
 	}
 }
 
@@ -1657,7 +1650,7 @@ void Platform::SetHeater(size_t heater, float power)
 	if (heatOnPins[heater] != NoPin)
 	{
 		uint16_t freq = (reprap.GetHeat()->UseSlowPwm(heater)) ? SlowHeaterPwmFreq : NormalHeaterPwmFreq;
-		AnalogOut(heatOnPins[heater], (HEAT_ON) ? power : 1.0 - power, freq);
+		WriteAnalog(heatOnPins[heater], (HEAT_ON) ? power : 1.0 - power, freq);
 	}
 }
 
@@ -1691,17 +1684,26 @@ void Platform::UpdateConfiguredHeaters()
 
 EndStopHit Platform::Stopped(size_t drive) const
 {
-	if (endStopType[drive] == EndStopType::noEndStop)
+	if (drive < DRIVES && endStopPins[drive] != NoPin)
 	{
-		// No homing switch is configured for this axis, so see if we should use the Z probe
-		if (nvData.zProbeType > 0 && drive < reprap.GetGCodes()->GetNumAxes() && (nvData.zProbeAxes & (1 << drive)) != 0)
+		if (drive >= reprap.GetGCodes()->GetNumAxes())
 		{
-			return GetZProbeResult();			// using the Z probe as a low homing stop for this axis, so just get its result
+			// Endstop not used for an axis, so no configuration data available.
+			// To allow us to see its status in DWC, pretend it is configured as a high-end active high endstop.
+			if (ReadPin(endStopPins[drive]))
+			{
+				return EndStopHit::highHit;
+			}
 		}
-	}
-	else if (endStopPins[drive] != NoPin)
-	{
-		if (digitalRead(endStopPins[drive]) == endStopLogicLevel[drive])
+		else if (endStopType[drive] == EndStopType::noEndStop)
+		{
+			// No homing switch is configured for this axis, so see if we should use the Z probe
+			if (nvData.zProbeType > 0 && drive < reprap.GetGCodes()->GetNumAxes() && (nvData.zProbeAxes & (1 << drive)) != 0)
+			{
+				return GetZProbeResult();			// using the Z probe as a low homing stop for this axis, so just get its result
+			}
+		}
+		else if (ReadPin(endStopPins[drive]) == endStopLogicLevel[drive])
 		{
 			return (endStopType[drive] == EndStopType::highEndStop) ? EndStopHit::highHit : EndStopHit::lowHit;
 		}
@@ -1716,7 +1718,7 @@ uint32_t Platform::GetAllEndstopStates() const
 	for (unsigned int drive = 0; drive < DRIVES; ++drive)
 	{
 		const Pin pin = endStopPins[drive];
-		if (pin != NoPin && digitalRead(pin))
+		if (pin != NoPin && ReadPin(pin))
 		{
 			rslt |= (1 << drive);
 		}
@@ -1728,10 +1730,7 @@ uint32_t Platform::GetAllEndstopStates() const
 EndStopHit Platform::GetZProbeResult() const
 {
 	const int zProbeVal = ZProbe();
-	const int zProbeADValue =
-			(nvData.zProbeType == 4 || nvData.zProbeType == 5) ? nvData.switchZProbeParameters.adcValue
-			: (nvData.zProbeType == 3 || nvData.zProbeType == 6) ? nvData.alternateZProbeParameters.adcValue
-				: nvData.irZProbeParameters.adcValue;
+	const int zProbeADValue = GetZProbeParameters().adcValue;
 	return (zProbeVal >= zProbeADValue) ? EndStopHit::lowHit
 			: (zProbeVal * 10 >= zProbeADValue * 9) ? EndStopHit::lowNear	// if we are at/above 90% of the target value
 				: EndStopHit::noStop;
@@ -2112,6 +2111,17 @@ void Platform::SetFanValue(size_t fan, float speed)
 	}
 }
 
+#if !defined(DUET_NG) && !defined(__RADDS__)
+
+// Enable or disable the fan that shares its PWM pin with the last heater. Called when we disable or enable the last heater.
+void Platform::EnableSharedFan(bool enable)
+{
+	fans[NUM_FANS - 1].Init((enable) ? COOLING_FAN_PINS[NUM_FANS - 1] : NoPin, FansHardwareInverted());
+}
+
+#endif
+
+
 // Get current fan RPM
 float Platform::GetFanRPM()
 {
@@ -2124,18 +2134,21 @@ float Platform::GetFanRPM()
 			: 0.0;															// else assume fan is off or tacho not connected
 }
 
-void Platform::InitFans()
+bool Platform::FansHardwareInverted() const
 {
-	for (size_t i = 0; i < NUM_FANS; ++i)
-	{
-		fans[i].Init(COOLING_FAN_PINS[i],
 #if defined(DUET_NG) || defined(__RADDS__)
-				false
+	return false;
 #else
-				// The cooling fan output pin gets inverted if HEAT_ON == 0 on a Duet 0.6 or 0.7
-				!HEAT_ON && (board == BoardType::Duet_06 || board == BoardType::Duet_07)
+	// The cooling fan output pin gets inverted on a Duet 0.6 or 0.7
+	return board == BoardType::Duet_06 || board == BoardType::Duet_07;
 #endif
-				);
+}
+
+void Platform::InitFans()
+{
+	for (size_t i = 0; i < NUM_FANS; ++i)
+	{
+		fans[i].Init(COOLING_FAN_PINS[i], FansHardwareInverted());
 	}
 
 	if (NUM_FANS > 1)
@@ -2424,12 +2437,12 @@ void Platform::MessageF(MessageType type, const char *fmt, ...)
 
 bool Platform::AtxPower() const
 {
-	return (digitalRead(ATX_POWER_PIN));
+	return ReadPin(ATX_POWER_PIN);
 }
 
 void Platform::SetAtxPower(bool on)
 {
-	digitalWrite(ATX_POWER_PIN, on);
+	WriteDigital(ATX_POWER_PIN, on);
 }
 
 
@@ -2579,20 +2592,86 @@ const char* Platform::GetBoardString() const
 	}
 }
 
-// Direct pin operations
-// Set the specified pin to the specified output level. Return true if success, false if not allowed.
-bool Platform::SetPin(int pin, float level)
+// User I/O and servo support
+bool Platform::GetFirmwarePin(int logicalPin, PinAccess access, Pin& firmwarePin, bool& invert)
 {
-	if (pin >= 0 && (unsigned int)pin < NUM_PINS_ALLOWED)
+#ifdef __RADDS__
+# error This needs to be modified for RADDS
+#endif
+	firmwarePin = NoPin;										// assume failure
+	invert = false;												// this is the common case
+	if (logicalPin < 0 || logicalPin > HighestLogicalPin)
+	{
+		// Pin number out of range, so nothing to do here
+	}
+	else if (logicalPin < HEATERS)								// pins 0-9 correspond to heater channels
 	{
-		const size_t index = (unsigned int)pin/8;
-		const uint8_t mask = 1 << ((unsigned int)pin & 7);
-		if ((pinAccessAllowed[index] & mask) != 0)
+		// For safety, we don't allow a heater channel to be used for servos until the heater has been disabled
+		if (!reprap.GetHeat()->IsHeaterEnabled(logicalPin))
 		{
-			AnalogOut(pin, level);
-			return true;
+			firmwarePin = heatOnPins[logicalPin];
+			invert = !HEAT_ON;
 		}
 	}
+	else if (logicalPin >= 20 && logicalPin < 20 + (int)NUM_FANS)	// pins 100-107 correspond to fan channels
+	{
+		// Don't allow a fan channel to be used unless the fan has been disabled
+		if (!fans[logicalPin - 20].IsEnabled()
+#ifdef DUET_NG
+			// Fan pins on the DueX2/DueX5 cannot be used to control servos because the frequency is not well-defined.
+			&& (logicalPin <= 22 || access != PinAccess::servo)
+#endif
+		   )
+		{
+			firmwarePin = COOLING_FAN_PINS[logicalPin - 20];
+		}
+	}
+	else if (logicalPin >= 40 && logicalPin < 40 + (int)ARRAY_SIZE(endStopPins))	// pins 40-49 correspond to endstop pins
+	{
+		if (access == PinAccess::read
+#ifdef DUET_NG
+			// Endstop pins on the DueX2/DueX5 can be used as digital outputs too
+			|| (access == PinAccess::write && logicalPin >= 45)
+#endif
+		   )
+		{
+			firmwarePin = endStopPins[logicalPin - 40];
+		}
+	}
+	else if (logicalPin >= 60 && logicalPin < 60 + (int)ARRAY_SIZE(SpecialPinMap))
+	{
+		firmwarePin = SpecialPinMap[logicalPin - 60];
+	}
+#ifdef DUET_NG
+	else if (logicalPin >= 100 && logicalPin <= 103)			// Pins 100-103 are the GPIO pins on the DueX2/X5
+	{
+		static const Pin DueX5GpioPinMap[] = { 211, 210, 209, 208 };
+		if (access != PinAccess::servo)
+		{
+			firmwarePin = DueX5GpioPinMap[logicalPin - 100];
+		}
+	}
+#endif
+
+	if (firmwarePin != NoPin)
+	{
+		// Check that the pin mode has been defined suitably
+		PinMode desiredMode;
+		if (access == PinAccess::write || access == PinAccess::servo)
+		{
+			desiredMode = (firmwarePin >= 100) ? OUTPUT_PWM : OUTPUT_LOW;
+		}
+		else
+		{
+			desiredMode = INPUT_PULLUP;
+		}
+		if (logicalPinModes[logicalPin] != (int8_t)desiredMode)
+		{
+			SetPinMode(firmwarePin, desiredMode);
+			logicalPinModes[logicalPin] = (int8_t)desiredMode;
+		}
+		return true;
+	}
 	return false;
 }
 
diff --git a/src/Platform.h b/src/Platform.h
index 6f35665d..6ec5f37a 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -51,7 +51,7 @@ Licence: GPL
 #include "Libraries/Fatfs/ff.h"
 
 #if defined(DUET_NG)
-# include "SX1509B.h"
+# include "DueXn.h"
 #else
 # include "Libraries/MCP4461/MCP4461.h"
 #endif
@@ -110,7 +110,7 @@ const float defaultDeltaHomedHeight = 200;						// mm
 
 const float Z_PROBE_STOP_HEIGHT = 0.7;							// Millimetres
 const unsigned int Z_PROBE_AVERAGE_READINGS = 8;				// We average this number of readings with IR on, and the same number with IR off
-const int ZProbeTypeDelta = 6;									// Z probe type for experimental delta probe
+const int ZProbeTypeDelta = 7;									// Z probe type for experimental delta probe
 
 #ifdef DUET_NG
 const int Z_PROBE_AD_VALUE = 500;								// Default for the Z probe - should be overwritten by experiment
@@ -237,6 +237,14 @@ enum class DiagnosticTestType : int
 	PrintMoves = 100				// print summary of recent moves
 };
 
+// Enumeration to describe what we want to do with a logical pin
+enum class PinAccess : int
+{
+	read,
+	write,
+	servo
+};
+
 // Info returned by FindFirst/FindNext calls
 class FileInfo
 {
@@ -586,7 +594,7 @@ public:
 	void SetZProbeAxes(uint32_t axes);
 	uint32_t GetZProbeAxes() const { return nvData.zProbeAxes; }
 	const ZProbeParameters& GetZProbeParameters() const;
-	bool SetZProbeParameters(const struct ZProbeParameters& params);
+	void SetZProbeParameters(const struct ZProbeParameters& params);
 	bool MustHomeXYBeforeZ() const;
 
 	void SetExtrusionAncilliaryPWM(float v);
@@ -615,7 +623,7 @@ public:
 	bool AnyHeaterHot(uint16_t heaters, float t);			// called to see if we need to turn on the hot end fan
 
 	// Fans
-	Fan& GetFan(size_t fanNumber)									// Get access to the fan control object
+	Fan& GetFan(size_t fanNumber)							// Get access to the fan control object
 	pre(fanNumber < NUM_FANS)
 	{
 		return fans[fanNumber];
@@ -623,6 +631,9 @@ public:
 
 	float GetFanValue(size_t fan) const;					// Result is returned in percent
 	void SetFanValue(size_t fan, float speed);				// Accepts values between 0..1 and 1..255
+#ifndef DUET_NG
+	void EnableSharedFan(bool enable);						// enable/disable the fan that shares its PWM pin with the last heater
+#endif
 	float GetFanRPM();
 
 	// Flash operations
@@ -649,19 +660,25 @@ public:
 	// So you can test for inkjet presence with if(platform->Inkjet(0))
 	bool Inkjet(int bitPattern);
 
-	// Direct pin operations
-	bool SetPin(int pin, float level);
-
 	// MCU temperature
 	void GetMcuTemperatures(float& minT, float& currT, float& maxT) const;
 	void SetMcuTemperatureAdjust(float v) { mcuTemperatureAdjust = v; }
 	float GetMcuTemperatureAdjust() const { return mcuTemperatureAdjust; }
 
+	// Low level port access
+	static void SetPinMode(Pin p, PinMode mode);
+	static bool ReadPin(Pin p);
+	static void WriteDigital(Pin p, bool high);
+	static void WriteAnalog(Pin p, float pwm, uint16_t frequency);
+
 #ifdef DUET_NG
 	// Power in voltage
 	void GetPowerVoltages(float& minV, float& currV, float& maxV) const;
 #endif
 
+	// User I/O and servo support
+	bool GetFirmwarePin(int logicalPin, PinAccess access, Pin& firmwarePin, bool& invert);
+
 //-------------------------------------------------------------------------------------------------------
   
 private:
@@ -759,7 +776,6 @@ private:
 	float maxAverageAcceleration;
 
 #if defined(DUET_NG)
-	SX1509B expansion;								// I/O expander on DueXn board
 	size_t numTMC2660Drivers;						// the number of TMC2660 drivers we have, the remaining are simple enable/step/dir drivers
 #else
 	// Digipots
@@ -808,6 +824,7 @@ private:
 	Pin coolingFanRpmPin;											// we currently support only one fan RPM input
 	float lastRpmResetTime;
 	void InitFans();
+	bool FansHardwareInverted() const;
 
   	// Serial/USB
 
@@ -892,8 +909,7 @@ private:
 #endif
 
 	// Direct pin manipulation
-	static const uint8_t pinAccessAllowed[NUM_PINS_ALLOWED/8];
-	uint8_t pinInitialised[NUM_PINS_ALLOWED/8];
+	int8_t logicalPinModes[HighestLogicalPin + 1];		// what mode each logical pin is set to - would ideally be class PinMode not int8_t
 };
 
 // Small class to hold an open file and data relating to it.
@@ -997,6 +1013,70 @@ private:
 	FileData(const FileData&);
 };
 
+/*static*/ inline void Platform::SetPinMode(Pin pin, PinMode mode)
+{
+#ifdef DUET_NG
+	if (pin >= ExpansionStart)
+	{
+		DuetExpansion::SetPinMode(pin - ExpansionStart, mode);
+	}
+	else
+	{
+		pinMode(pin, mode);
+	}
+#else
+	pinMode(pin, mode);
+#endif
+}
+
+/*static*/ inline bool Platform::ReadPin(Pin pin)
+{
+#ifdef DUET_NG
+	if (pin >= ExpansionStart)
+	{
+		return DuetExpansion::DigitalRead(pin - ExpansionStart);
+	}
+	else
+	{
+		return digitalRead(pin);
+	}
+#else
+	return digitalRead(pin);
+#endif
+}
+
+/*static*/ inline void Platform::WriteDigital(Pin pin, bool high)
+{
+#ifdef DUET_NG
+	if (pin >= ExpansionStart)
+	{
+		DuetExpansion::DigitalWrite(pin - ExpansionStart, high);
+	}
+	else
+	{
+		digitalWrite(pin, high);
+	}
+#else
+	digitalWrite(pin, high);
+#endif
+}
+
+/*static*/ inline void Platform::WriteAnalog(Pin pin, float pwm, uint16_t freq)
+{
+#ifdef DUET_NG
+	if (pin >= ExpansionStart)
+	{
+		DuetExpansion::AnalogOut(pin - ExpansionStart, pwm);
+	}
+	else
+	{
+		AnalogOut(pin, pwm, freq);
+	}
+#else
+	AnalogOut(pin, pwm);
+#endif
+}
+
 // Where the htm etc files are
 
 inline const char* Platform::GetWebDir() const
@@ -1265,7 +1345,7 @@ inline uint16_t Platform::GetRawZProbeReading() const
 	{
 	case 4:
 		{
-			bool b = digitalRead(endStopPins[E0_AXIS]);
+			bool b = ReadPin(endStopPins[E0_AXIS]);
 			if (!endStopLogicLevel[MAX_AXES])
 			{
 				b = !b;
@@ -1274,7 +1354,7 @@ inline uint16_t Platform::GetRawZProbeReading() const
 		}
 
 	case 5:
-		return (digitalRead(zProbePin)) ? 4000 : 0;
+		return (ReadPin(zProbePin)) ? 4000 : 0;
 
 	default:
 		return AnalogInReadChannel(zProbeAdcChannel);
diff --git a/src/Reprap.cpp b/src/Reprap.cpp
index 196370ee..00ddbaca 100644
--- a/src/Reprap.cpp
+++ b/src/Reprap.cpp
@@ -946,8 +946,15 @@ OutputBuffer *RepRap::GetConfigResponse()
 	}
 
 	// Firmware details
-	response->catf("],\"firmwareElectronics\":\"%s\"", platform->GetElectronicsString());
-	response->catf(",\"firmwareName\":\"%s\"", NAME);
+	response->catf("],\"firmwareElectronics\":\"%s", platform->GetElectronicsString());
+#ifdef DUET_NG
+	const char* expansionName = DuetExpansion::GetExpansionBoardName();
+	if (expansionName != nullptr)
+	{
+		response->catf(" + %s", expansionName);
+	}
+#endif
+	response->catf("\",\"firmwareName\":\"%s\"", NAME);
 	response->catf(",\"firmwareVersion\":\"%s\"", VERSION);
 #ifdef DUET_NG
 	response->catf(",\"dwsVersion\":\"%s\"", network->GetWiFiServerVersion());