From 8ed3bbd8451b1455130aa60ca6d01436a2fc3c40 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Thu, 16 Feb 2023 16:41:13 +0000
Subject: [PATCH] Support for multiple displays

Refactor display handling so DisplayInterface class passes the relevant commands to the relevant display objects.
---
 CommandStation-EX.ino    |  12 +-
 Display.cpp              |  70 ++++---
 Display.h                |  62 +++---
 DisplayInterface.cpp     |   5 +-
 DisplayInterface.h       |  84 ++++++--
 Display_Implementation.h |  23 ++-
 IO_OLEDDisplay.h         | 212 +++++++++-----------
 LiquidCrystal_I2C.cpp    |  38 ++--
 LiquidCrystal_I2C.h      |  15 +-
 SSD1306Ascii.cpp         | 411 ++++++++++++++++++++++++++-------------
 SSD1306Ascii.h           |  32 +--
 StringFormatter.cpp      |  12 +-
 defines.h                |   4 +
 13 files changed, 598 insertions(+), 382 deletions(-)

diff --git a/CommandStation-EX.ino b/CommandStation-EX.ino
index 1f57dc4..3c40bba 100644
--- a/CommandStation-EX.ino
+++ b/CommandStation-EX.ino
@@ -49,6 +49,7 @@
  */
 
 #include "DCCEX.h"
+#include "Display_Implementation.h"
 
 #ifdef CPU_TYPE_ERROR
 #error CANNOT COMPILE - DCC++ EX ONLY WORKS WITH THE ARCHITECTURES LISTED IN defines.h
@@ -74,11 +75,11 @@ void setup()
 
   DIAG(F("License GPLv3 fsf.org (c) dcc-ex.com"));
 
-  CONDITIONAL_DISPLAY_START {
-    // This block is still executed for DIAGS if LCD not in use
-    LCD(0,F("DCC++ EX v%S"),F(VERSION));
+  DISPLAY_START (
+    // This block is still executed for DIAGS if display not in use
+    LCD(0,F("DCC-EX v%S"),F(VERSION));
     LCD(1,F("Lic GPLv3"));
-  }
+  );
 
   // Responsibility 2: Start all the communications before the DCC engine
   // Start the WiFi interface on a MEGA, Uno cannot currently handle WiFi
@@ -160,7 +161,8 @@ void loop()
   LCN::loop();
   #endif
 
-  Display::loop();  // ignored if LCD not in use
+  // Display refresh
+  DisplayInterface::loop();
 
   // Handle/update IO devices.
   IODevice::loop();
diff --git a/Display.cpp b/Display.cpp
index 5af85b8..587a107 100644
--- a/Display.cpp
+++ b/Display.cpp
@@ -47,42 +47,65 @@
 
 #include "Display.h"
 
-void Display::clear() {
-  clearNative();
-  for (byte row = 0; row < MAX_LCD_ROWS; row++) rowBuffer[row][0] = '\0';
+// Constructor - allocates device driver.
+Display::Display(DisplayDevice *deviceDriver) {
+  _deviceDriver = deviceDriver;
+  // Get device dimensions in characters (e.g. 16x2).
+  numCharacterColumns = _deviceDriver->getNumCols();
+  numCharacterRows = _deviceDriver->getNumRows();;
+  for (uint8_t row=0; row<MAX_CHARACTER_ROWS; row++)
+    rowBuffer[row] = (char *)calloc(1, MAX_CHARACTER_COLS+1);
+  topRow = -1;  // loop2 will fill from row 0
+  
+  addDisplay(0);  // Add this display as display number 0
+};
+
+void Display::begin() {
+  _deviceDriver->begin();
+  _deviceDriver->clearNative();
+}
+
+void Display::_clear() {
+  _deviceDriver->clearNative();
+  for (uint8_t row = 0; row < MAX_CHARACTER_ROWS; row++) 
+    rowBuffer[row][0] = '\0';
   topRow = -1;  // loop2 will fill from row 0
 }
 
-void Display::setRow(byte line) {
+void Display::_setRow(uint8_t line) {
   hotRow = line;
   hotCol = 0;
+  rowBuffer[hotRow][hotCol] = 0;  // Clear existing text
 }
 
-size_t Display::write(uint8_t b) {
-  if (hotRow >= MAX_LCD_ROWS || hotCol >= MAX_LCD_COLS) return -1;
+size_t Display::_write(uint8_t b) {
+  if (hotRow >= MAX_CHARACTER_ROWS || hotCol >= MAX_CHARACTER_COLS) return -1;
   rowBuffer[hotRow][hotCol] = b;
   hotCol++;
   rowBuffer[hotRow][hotCol] = 0;
   return 1;
 }
 
-void Display::loop() {
-  if (!displayHandler) return;
-  displayHandler->loop2(false);
+// Refresh screen completely (will block until complete). Used
+// during start-up.
+void Display::_refresh() {
+  loop2(true);
+}
+
+// On normal loop entries, loop will only make one output request on each
+// entry, to avoid blocking while waiting for the I2C.
+void Display::_displayLoop() {
+  // If output device is busy, don't do anything on this loop
+  // This avoids blocking while waiting for the device to complete.
+  if (!_deviceDriver->isBusy()) loop2(false);
 }
 
 Display *Display::loop2(bool force) {
-  if (!displayHandler) return NULL;
-
-  // If output device is busy, don't do anything on this loop
-  //  This avoids blocking while waiting for the device to complete.
-  if (isBusy()) return NULL;  
-
   unsigned long currentMillis = millis();
 
   if (!force) {
     // See if we're in the time between updates
-    if ((currentMillis - lastScrollTime) < LCD_SCROLL_TIME)
+    if ((currentMillis - lastScrollTime) < DISPLAY_SCROLL_TIME)
       return NULL;
   } else {
     // force full screen update from the beginning.
@@ -104,7 +127,7 @@ Display *Display::loop2(bool force) {
           buffer[i] = rowBuffer[rowNext][i];
       } else
         buffer[0] = '\0';  // Empty line
-      setRowNative(slot);  // Set position for display
+      _deviceDriver->setRowNative(slot);  // Set position for display
       charIndex = 0;
       bufferPointer = &buffer[0];
 
@@ -113,12 +136,12 @@ Display *Display::loop2(bool force) {
       // Write next character, or a space to erase current position.
       char ch = *bufferPointer;
       if (ch) {
-        writeNative(ch);
+        _deviceDriver->writeNative(ch);
         bufferPointer++;
       } else
-        writeNative(' ');
+        _deviceDriver->writeNative(' ');
 
-      if (++charIndex >= MAX_LCD_COLS) {
+      if (++charIndex >= MAX_CHARACTER_COLS) {
         // Screen slot completed, move to next slot on screen
         slot++;
         bufferPointer = 0;
@@ -128,7 +151,7 @@ Display *Display::loop2(bool force) {
         }
       }
 
-      if (slot >= lcdRows) {
+      if (slot >= numCharacterRows) {
         // Last slot finished, reset ready for next screen update.
 #if SCROLLMODE==2
         if (!done) {
@@ -151,7 +174,8 @@ Display *Display::loop2(bool force) {
 }
 
 void Display::moveToNextRow() {
-  rowNext = (rowNext + 1) % MAX_LCD_ROWS;
+  rowNext = rowNext + 1;
+  if (rowNext >= MAX_CHARACTER_ROWS) rowNext = 0;
 #if SCROLLMODE == 1
   // Finished if we've looped back to row 0
   if (rowNext == 0) done = true;
@@ -164,4 +188,4 @@ void Display::moveToNextRow() {
 void Display::skipBlankRows() {
   while (!done && rowBuffer[rowNext][0] == 0)
     moveToNextRow();
-}
+}
\ No newline at end of file
diff --git a/Display.h b/Display.h
index 84cdbde..7bbbc78 100644
--- a/Display.h
+++ b/Display.h
@@ -16,15 +16,18 @@
  *  You should have received a copy of the GNU General Public License
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
-#ifndef LCDDisplay_h
-#define LCDDisplay_h
+#ifndef Display_h
+#define Display_h
 #include <Arduino.h>
 #include "defines.h"
 #include "DisplayInterface.h"
 
 // Allow maximum message length to be overridden from config.h
 #if !defined(MAX_MSG_SIZE) 
-#define MAX_MSG_SIZE 20
+// On a screen that's 128 pixels wide, character 22 overlaps end of screen
+// However, by making the line longer than the screen, we don't have to 
+// clear the screen, we just overwrite what was there.
+#define MAX_MSG_SIZE 22  
 #endif
 
 // Set default scroll mode (overridable in config.h)
@@ -32,36 +35,17 @@
 #define SCROLLMODE 1
 #endif
 
-// This class is created in LCDisplay_Implementation.h
+// This class is created in Display_Implementation.h
 
 class Display : public DisplayInterface {
- public:
-  Display() {};
-  static const int MAX_LCD_ROWS = 8;
-  static const int MAX_LCD_COLS = MAX_MSG_SIZE;
-  static const long LCD_SCROLL_TIME = 3000;  // 3 seconds
+public:
+  Display(DisplayDevice *deviceDriver);
+  static const int MAX_CHARACTER_ROWS = 8;
+  static const int MAX_CHARACTER_COLS = MAX_MSG_SIZE;
+  static const long DISPLAY_SCROLL_TIME = 3000;  // 3 seconds
 
-  // Internally handled functions
-  static void loop();
-  Display* loop2(bool force) override;
-  void setRow(byte line) override;
-  void clear() override;
-
-  size_t write(uint8_t b) override;
-
-protected:
-  uint8_t lcdRows;
-  uint8_t lcdCols;
-
- private:
-  void moveToNextRow();
-  void skipBlankRows();
-
-  // Relay functions to the live driver in the subclass
-  virtual void clearNative() = 0;
-  virtual void setRowNative(byte line) = 0;
-  virtual size_t writeNative(uint8_t b) = 0;
-  virtual bool isBusy() = 0;
+private:
+  DisplayDevice *_deviceDriver;
 
   unsigned long lastScrollTime = 0;
   int8_t hotRow = 0;
@@ -71,11 +55,25 @@ protected:
   int8_t rowFirst = -1;
   int8_t rowNext = 0;
   int8_t charIndex = 0;
-  char buffer[MAX_LCD_COLS + 1];
+  char buffer[MAX_CHARACTER_COLS + 1];
   char* bufferPointer = 0;
   bool done = false;
+  uint16_t numCharacterRows;
+  uint16_t numCharacterColumns = MAX_CHARACTER_COLS;
+
+  char *rowBuffer[MAX_CHARACTER_ROWS];
+
+public:
+  void begin() override;  
+  void _clear() override;
+  void _setRow(uint8_t line) override;
+  size_t _write(uint8_t b) override;
+  void _refresh() override;
+  void _displayLoop() override;
+  Display *loop2(bool force);
+  void moveToNextRow();
+  void skipBlankRows();
 
-  char rowBuffer[MAX_LCD_ROWS][MAX_LCD_COLS + 1];
 };
 
 #endif
diff --git a/DisplayInterface.cpp b/DisplayInterface.cpp
index 6ef13a4..f2c144e 100644
--- a/DisplayInterface.cpp
+++ b/DisplayInterface.cpp
@@ -21,4 +21,7 @@
 
 #include "DisplayInterface.h"
 
-DisplayInterface *DisplayInterface::displayHandler = 0;
+// Install null display driver initially - will be replaced if required.
+DisplayInterface *DisplayInterface::_displayHandler = new DisplayInterface();
+
+uint8_t DisplayInterface::_selectedDisplayNo = 255;
diff --git a/DisplayInterface.h b/DisplayInterface.h
index 82b38d6..f1d598d 100644
--- a/DisplayInterface.h
+++ b/DisplayInterface.h
@@ -25,23 +25,75 @@
 
 // Definition of base class for displays.  The base class does nothing.
 class DisplayInterface : public Print {
-public:
-  virtual DisplayInterface* loop2(bool force) { (void)force; return NULL; };
-  virtual void setRow(byte line) { (void)line; };
-  virtual void clear() { };
-  virtual size_t write(uint8_t c) { (void)c; return 0; };
-  // Additional functions to support multiple displays.
-  // Display number zero is the default one and the original display
-  // drivers overloaded the above calls only.  Newer display drivers
-  // (e.g. HAL IO_OledDisplay) should override all functions.
-  virtual void setRow(uint8_t displayNo, byte line) { 
-    if (!displayNo) setRow(line);
-  }
-  virtual void clear(uint8_t displayNo) { 
-    if (!displayNo) clear();
-   }
+protected:
+  static DisplayInterface *_displayHandler;
+  static uint8_t _selectedDisplayNo;  // Nothing selected.
+  DisplayInterface *_nextHandler = NULL;
+  uint8_t _displayNo = 0;
 
-  static DisplayInterface *displayHandler;
+public:
+  // Add display object to list of displays
+  void addDisplay(uint8_t displayNo) {
+    _nextHandler = _displayHandler;
+    _displayHandler = this;
+    _displayNo = displayNo;
+  }
+  static DisplayInterface *getDisplayHandler() {
+    return _displayHandler;
+  }
+  uint8_t getDisplayNo() {
+    return _displayNo;
+  }
+
+  // The next functions are to provide compatibility with calls to the LCD function
+  // which does not specify a display number.  These always apply to display '0'.
+  static void refresh() { refresh(0); };
+  static void setRow(uint8_t line) { setRow(0, line); };
+  static void clear() { clear(0); };
+
+  // Additional functions to support multiple displays.  These perform a
+  // multicast to all displays that match the selected displayNo.
+  // Display number zero is the default one.
+  static void setRow(uint8_t displayNo, uint8_t line) { 
+    _selectedDisplayNo = displayNo;
+    for (DisplayInterface *p = _displayHandler; p!=0; p=p->_nextHandler) { 
+      if (displayNo == p->_displayNo) p->_setRow(line);
+    }
+  }
+  size_t write (uint8_t c) override {
+    for (DisplayInterface *p = _displayHandler; p!=0; p=p->_nextHandler) 
+      if (_selectedDisplayNo == p->_displayNo) p->_write(c);
+    return _displayHandler ? 1 : 0;
+  }
+  static void clear(uint8_t displayNo) { 
+    for (DisplayInterface *p = _displayHandler; p!=0; p=p->_nextHandler) 
+      if (displayNo == p->_displayNo) p->_clear();
+  }
+  static void refresh(uint8_t displayNo) {
+    for (DisplayInterface *p = _displayHandler; p!=0; p=p->_nextHandler)
+      if (displayNo == p->_displayNo) p->_refresh();
+  }
+  static void loop() {
+    for (DisplayInterface *p = _displayHandler; p!=0; p=p->_nextHandler) 
+      p->_displayLoop();
+  };
+  // The following are overridden within the specific device class
+  virtual void begin() {};
+  virtual size_t _write(uint8_t c) { (void)c; return 0; };
+  virtual void _setRow(uint8_t line) {}
+  virtual void _clear() {}
+  virtual void _refresh() {}
+  virtual void _displayLoop() {}
 };
 
+class DisplayDevice {
+public:
+  virtual bool begin() { return true; }
+  virtual void clearNative() = 0;
+  virtual void setRowNative(uint8_t line) = 0;
+  virtual size_t writeNative(uint8_t c) = 0;
+  virtual bool isBusy() = 0;
+  virtual uint16_t getNumRows() = 0;
+  virtual uint16_t getNumCols() = 0;
+};
 #endif
diff --git a/Display_Implementation.h b/Display_Implementation.h
index f04b7d2..ca19bd7 100644
--- a/Display_Implementation.h
+++ b/Display_Implementation.h
@@ -27,7 +27,7 @@
 
 #ifndef LCD_Implementation_h
 #define LCD_Implementation_h
-#include "Display.h"
+#include "DisplayInterface.h"
 #include "SSD1306Ascii.h"
 #include "LiquidCrystal_I2C.h"
   
@@ -35,19 +35,26 @@
 // Implement the Display shim class as a singleton.
 // The DisplayInterface class implements a display handler with no code (null device);
 // The Display class sub-classes DisplayInterface to provide the common display code;
-// Then Display class is subclassed to the specific device type classes:
+// Then Display class talks to the specific device type classes:
 //    SSD1306AsciiWire for I2C OLED driver with SSD1306 or SH1106 controllers;
 //    LiquidCrystal_I2C for I2C LCD driver for HD44780 with PCF8574 'backpack'.
 
 #if defined(OLED_DRIVER)
-  #define CONDITIONAL_DISPLAY_START for (DisplayInterface * dummy=new SSD1306AsciiWire(OLED_DRIVER);dummy!=NULL; dummy=dummy->loop2(true))
+  #define DISPLAY_START(xxx) { \
+    DisplayInterface *t = new Display(new SSD1306AsciiWire(OLED_DRIVER)); \
+    t->begin(); \
+    xxx; \
+    t->refresh(); \
+  } 
   
 #elif defined(LCD_DRIVER)
-  #define CONDITIONAL_DISPLAY_START for (DisplayInterface * dummy=new LiquidCrystal_I2C(LCD_DRIVER);dummy!=NULL; dummy=dummy->loop2(true))
-
+  #define DISPLAY_START(xxx) { \
+    DisplayInterface *t = new Display(new LiquidCrystal_I2C(LCD_DRIVER)); \
+    t->begin(); \
+    xxx;  \
+    t->refresh();}
 #else
-  // Create null display handler just in case someone calls displayHandler->something without checking if displayHandler is NULL!
-  #define CONDITIONAL_DISPLAY_START { new DisplayInterface(); }
-#endif
+  #define DISPLAY_START(xxx) {}
 
+#endif
 #endif // LCD_Implementation_h
diff --git a/IO_OLEDDisplay.h b/IO_OLEDDisplay.h
index 12b571f..26c24b3 100644
--- a/IO_OLEDDisplay.h
+++ b/IO_OLEDDisplay.h
@@ -49,13 +49,15 @@
 #include "SSD1306Ascii.h"
 #include "version.h"
 
-class OLEDDisplay : public IODevice, DisplayInterface {
+typedef SSD1306AsciiWire OLED;
+
+template <class T> 
+class OLEDDisplay : public IODevice, public DisplayInterface {
 private:
-  uint8_t _displayNo = 0;
   // Here we define the device-specific variables.  
   uint8_t _height; // in pixels
   uint8_t _width;  // in pixels
-  SSD1306AsciiWire *oled;
+  T *_displayDriver;
   uint8_t _rowNo = 0;   // Row number being written by caller
   uint8_t _colNo = 0;  // Position in line being written by caller
   uint8_t _numRows;
@@ -66,26 +68,25 @@ private:
   uint8_t _rowNoToScreen = 0; 
   uint8_t _charPosToScreen = 0;
   DisplayInterface *_nextDisplay = NULL;
-  uint8_t _selectedDisplayNo = 0;
 
 public:
   //  Static function to handle "OLEDDisplay::create(...)" calls.
-  static void create(I2CAddress i2cAddress, int width = 128, int height=64) {
+  static void create(I2CAddress i2cAddress, int width, int height) {
     /* if (checkNoOverlap(i2cAddress)) */ new OLEDDisplay(0, i2cAddress, width, height);
   } 
-  static void create(uint8_t displayNo, I2CAddress i2cAddress, int width = 128, int height=64) {
+  static void create(uint8_t displayNo, I2CAddress i2cAddress, int width, int height) {
     /* if (checkNoOverlap(i2cAddress)) */ new OLEDDisplay(displayNo, i2cAddress, width, height);
   } 
 
 protected:
   // Constructor
   OLEDDisplay(uint8_t displayNo, I2CAddress i2cAddress, int width, int height) {
-    _displayNo = displayNo;
+    _displayDriver = new T(i2cAddress, width, height);
     _I2CAddress = i2cAddress;
     _width = width;
     _height = height;
-    _numCols = (_width+5) / 6;    // character block 6 x 8, round up 
-    _numRows = _height / 8;       // Round down
+    _numCols = _displayDriver->getNumCols();
+    _numRows = _displayDriver->getNumRows();
 
     _charPosToScreen = _numCols;
 
@@ -96,51 +97,28 @@ protected:
     // Fill buffer with spaces
     memset(_buffer, ' ', _numCols*_numRows);
 
+    _displayDriver->clearNative();
+
+    // Add device to list of HAL devices (not necessary but allows
+    // status to be displayed using <D HAL SHOW> and device to be
+    // reinitialised using <D HAL RESET>).
+    IODevice::addDevice(this);
+
+    // Also add this display to list of display handlers
+    DisplayInterface::addDisplay(displayNo);
+
     // Is this the main display?
-    if (_displayNo == 0) {
+    if (displayNo == 0) {
       // Set first two lines on screen
-      setRow(0);
-      print(F("DCC++ EX v"));
+      this->setRow(displayNo, 0);
+      print(F("DCC-EX v"));
       print(F(VERSION));
-      setRow(1);
+      setRow(displayNo, 1);
       print(F("Lic GPLv3"));
     }
-
-    // Create OLED driver
-    oled = new SSD1306AsciiWire();
-   
-    // Store pointer to this object into CS display hook, so that we
-    // will intercept any subsequent calls to displayHandler methods.
-    // Make a note of the existing display reference, to that we can
-    // pass on anything we're not interested in.
-    _nextDisplay = DisplayInterface::displayHandler;
-    DisplayInterface::displayHandler = this;
-
-    addDevice(this);
   }
   
-  // Device-specific initialisation
-  void _begin() override {
-    // Initialise device
-    if (oled->begin(_I2CAddress, _width, _height)) {
-
-      DIAG(F("OLEDDisplay installed on address %s"), _I2CAddress.toString());
-
-
-      // Force all rows to be redrawn
-      for (uint8_t row=0; row<_numRows; row++)
-        _rowGeneration[row]++;
-      
-      // Start with top line (looks better)
-      _rowNoToScreen = _numRows;
-      _charPosToScreen = _numCols;
-    }
-  }
-
-  void _loop(unsigned long) override {
-    //screenUpdate();
-  }
-
+  
   void screenUpdate() {
     // Loop through the buffer and if a row has changed
     // (rowGeneration[row] is changed) then start writing the
@@ -149,8 +127,8 @@ protected:
 
     // First check if the OLED driver is still busy from a previous 
     // call.  If so, don't to anything until the next entry.
-    if (!oled->isBusy()) {
-      // Check if we've just done the end of a row or just started
+    if (!_displayDriver->isBusy()) {
+      // Check if we've just done the end of a row
       if (_charPosToScreen >= _numCols) {
         // Move to next line
         if (++_rowNoToScreen >= _numRows)
@@ -159,101 +137,103 @@ protected:
         if (_rowGeneration[_rowNoToScreen] != _lastRowGeneration[_rowNoToScreen]) {
           // Row content has changed, so start outputting it
           _lastRowGeneration[_rowNoToScreen] = _rowGeneration[_rowNoToScreen];
-          oled->setRowNative(_rowNoToScreen);
+          _displayDriver->setRowNative(_rowNoToScreen);
           _charPosToScreen = 0;  // Prepare to output first character on next entry
         } else {
           // Row not changed, don't bother writing it.
         }
       } else {
         // output character at current position
-        oled->writeNative(_buffer[_rowNoToScreen*_numCols+_charPosToScreen++]);
+        _displayDriver->writeNative(_buffer[_rowNoToScreen*_numCols+_charPosToScreen++]);
       }  
     }
     return;
   }
+
+  /////////////////////////////////////////////////
+  // IODevice Class Member Overrides
+  /////////////////////////////////////////////////
+
+  // Device-specific initialisation
+  void _begin() override {
+    // Initialise device
+    if (_displayDriver->begin()) {
+
+      _display();
+
+      // Force all rows to be redrawn
+      for (uint8_t row=0; row<_numRows; row++)
+        _rowGeneration[row]++;
+      
+      // Start with top line (looks better).  
+      // The numbers will wrap round on the first loop2 entry.
+      _rowNoToScreen = _numRows;
+      _charPosToScreen = _numCols;
+    }
+  }
+
+  void _loop(unsigned long) override {
+    screenUpdate();
+  }
+  
+  // Display information about the device.
+  void _display() {
+    DIAG(F("OLEDDisplay %d configured on addr %s"), _displayNo, _I2CAddress.toString());
+  }
   
   /////////////////////////////////////////////////
   // DisplayInterface functions
   // 
   /////////////////////////////////////////////////
-  DisplayInterface* loop2(bool force) override {
+  
+public:
+  void _displayLoop() override {
     screenUpdate();
-    if (_nextDisplay) 
-      return _nextDisplay->loop2(force);  // continue to next display
-    return NULL;
   }
 
   // Position on nominated line number (0 to number of lines -1)
   // Clear the line in the buffer ready for updating
   // The displayNo referenced here is remembered and any following
   // calls to write() will be directed to that display.
-  void setRow(uint8_t displayNo, byte line) override {
-    _selectedDisplayNo = displayNo;
-    if (displayNo == _displayNo) {
-      if (line == 255) {
-        // LCD(255,"xxx") or SCREEN(displayNo,255, "xxx") - 
-        // scroll the contents of the buffer and put the new line
-        // at the bottom of the screen
-        for (int row=1; row<_numRows; row++) {
-          strncpy(&_buffer[(row-1)*_numCols], &_buffer[row*_numCols], _numCols);
-          _rowGeneration[row-1]++;
-        }
-        line = _numRows-1;
-      } else if (line >= _numRows) 
-        line = _numRows - 1;  // Overwrite bottom line.
-
-      _rowNo = line;
-      // Fill line with blanks
-      for (_colNo = 0; _colNo < _numCols; _colNo++)
-        _buffer[_rowNo*_numCols+_colNo] = ' ';
-      _colNo = 0;
-      // Mark that the buffer has been touched.  It will be 
-      // sent to the screen on the next loop entry, by which time
-      // the line should have been written to the buffer.
-      _rowGeneration[_rowNo]++;
-
-    } else if (_nextDisplay) 
-      _nextDisplay->setRow(displayNo, line); // Pass to next display
+  void _setRow(byte line) override {
+    if (line == 255) {
+      // LCD(255,"xxx") or SCREEN(displayNo,255, "xxx") - 
+      // scroll the contents of the buffer and put the new line
+      // at the bottom of the screen
+      for (int row=1; row<_numRows; row++) {
+        strncpy(&_buffer[(row-1)*_numCols], &_buffer[row*_numCols], _numCols);
+        _rowGeneration[row-1]++;
+      }
+      line = _numRows-1;
+    } else if (line >= _numRows) 
+      line = _numRows - 1;  // Overwrite bottom line.
 
+    _rowNo = line;
+    // Fill line with blanks
+    for (_colNo = 0; _colNo < _numCols; _colNo++)
+      _buffer[_rowNo*_numCols+_colNo] = ' ';
+    _colNo = 0;
+    // Mark that the buffer has been touched.  It will be 
+    // sent to the screen on the next loop entry, by which time
+    // the line should have been written to the buffer.
+    _rowGeneration[_rowNo]++;
   }
 
   // Write one character to the screen referenced in the last setRow() call.
-  size_t write(uint8_t c) override {
-    if (_selectedDisplayNo == _displayNo) {
-      // Write character to buffer (if there's space)
-      if (_colNo < _numCols) {
-        _buffer[_rowNo*_numCols+_colNo++] = c;
-      }
-      return 1;
-    } else if (_nextDisplay) 
-      return _nextDisplay->write(c);
-    else
-      return 0;
+  virtual size_t _write(uint8_t c) override {
+    // Write character to buffer (if there's space)
+    if (_colNo < _numCols) {
+      _buffer[_rowNo*_numCols+_colNo++] = c;
+    }
+    return 1;
   }
 
-  // Write blanks to all of the screen (blocks until complete)
-  void clear (uint8_t displayNo) override {
-    if (displayNo == _displayNo) {
-      // Clear buffer
-      for (_rowNo = 0; _rowNo < _numRows; _rowNo++) {
-        setRow(displayNo, _rowNo);
-      }
-      _rowNo = 0;
-    } else if (_nextDisplay)
-      _nextDisplay->clear(displayNo);  // Pass to next display
-  }
-  
-  // Overloads of above, for compatibility
-  void setRow(uint8_t line) override {
-    setRow(0, line);
-  }
-  void clear() override {
-    clear(0);
-  }
-
-  // Display information about the device.
-  void _display() {
-    DIAG(F("OLEDDisplay %d Configured addr %s"), _displayNo, _I2CAddress.toString());
+  // Write blanks to all of the screen buffer
+  void _clear() {
+    // Clear buffer
+    memset(_buffer, ' ', _numCols*_numRows);
+    _colNo = 0;
+    _rowNo = 0;
   }
 
 };
diff --git a/LiquidCrystal_I2C.cpp b/LiquidCrystal_I2C.cpp
index c9d8113..3bf98ef 100644
--- a/LiquidCrystal_I2C.cpp
+++ b/LiquidCrystal_I2C.cpp
@@ -44,24 +44,25 @@
 LiquidCrystal_I2C::LiquidCrystal_I2C(I2CAddress lcd_Addr, uint8_t lcd_cols,
                                      uint8_t lcd_rows) {
   _Addr = lcd_Addr;
-  lcdRows = lcd_rows;
-  lcdCols = lcd_cols;
-
+  lcdRows = lcd_rows;  // Number of character rows (typically 2 or 4).
+  lcdCols = lcd_cols;  // Number of character columns (typically 16 or 20)
   _backlightval = 0;
+ }
+
+bool LiquidCrystal_I2C::begin() {
 
   I2CManager.begin();
   I2CManager.setClock(100000L);    // PCF8574 is spec'd to 100kHz.
 
-  if (I2CManager.exists(lcd_Addr)) {
-    DIAG(F("%dx%d LCD configured on I2C:%s"), (int)lcd_cols, (int)lcd_rows, (int)lcd_Addr);
+  if (I2CManager.exists(_Addr)) {
+    DIAG(F("%dx%d LCD configured on I2C:%s"), (int)lcdCols, (int)lcdRows, _Addr.toString());
     _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
-    begin();
     backlight();
-    displayHandler = this;
+  } else {
+    DIAG(F("LCD not found on I2C:%s"), _Addr.toString());
+    return false;
   }
-}
 
-void LiquidCrystal_I2C::begin() {
   if (lcdRows > 1) {
     _displayfunction |= LCD_2LINE;
   }
@@ -99,26 +100,23 @@ void LiquidCrystal_I2C::begin() {
   _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;
   display();
 
-  // clear it off
-  clear();
-
   // Initialize to default text direction (for roman languages)
   _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT;
 
   // set the entry mode
   command(LCD_ENTRYMODESET | _displaymode);
 
-  setRowNative(0);
+  return true;
 }
 
 /********** high level commands, for the user! */
 void LiquidCrystal_I2C::clearNative() {
   command(LCD_CLEARDISPLAY);  // clear display, set cursor position to zero
-  delayMicroseconds(2000);    // this command takes 1.52ms
+  delayMicroseconds(1600);    // this command takes 1.52ms
 }
 
 void LiquidCrystal_I2C::setRowNative(byte row) {
-  int row_offsets[] = {0x00, 0x40, 0x14, 0x54};
+  uint8_t row_offsets[] = {0x00, 0x40, 0x14, 0x54};
   if (row >= lcdRows) {
     row = lcdRows - 1;  // we count rows starting w/0
   }
@@ -146,6 +144,10 @@ size_t LiquidCrystal_I2C::writeNative(uint8_t value) {
   return 1;
 }
 
+bool LiquidCrystal_I2C::isBusy() { 
+  return rb.isBusy();
+}
+
 /*********** mid level commands, for sending data/cmds */
 
 inline void LiquidCrystal_I2C::command(uint8_t value) { 
@@ -196,7 +198,7 @@ void LiquidCrystal_I2C::send(uint8_t value, uint8_t mode) {
   outputBuffer[len++] = highnib;
   outputBuffer[len++] = lownib|En;
   outputBuffer[len++] = lownib;
-  I2CManager.write(_Addr, outputBuffer, len);  // Write command synchronously
+  I2CManager.write(_Addr, outputBuffer, len, &rb);  // Write command asynchronously
 }
 
 // write 4 data bits to the HD44780 LCD controller.
@@ -208,12 +210,12 @@ void LiquidCrystal_I2C::write4bits(uint8_t value) {
   uint8_t len = 0;
   outputBuffer[len++] = _data|En;
   outputBuffer[len++] = _data;
-  I2CManager.write(_Addr, outputBuffer, len);  // Write command synchronously
+  I2CManager.write(_Addr, outputBuffer, len, &rb);  // Write command asynchronously
 }
 
 // write a byte to the PCF8574 I2C interface.  We don't need to set
 // the enable pin for this.
 void LiquidCrystal_I2C::expanderWrite(uint8_t value) {
   outputBuffer[0] = value | _backlightval;
-  I2CManager.write(_Addr, outputBuffer, 1);  // Write command synchronously
+  I2CManager.write(_Addr, outputBuffer, 1, &rb);  // Write command asynchronously
 }
\ No newline at end of file
diff --git a/LiquidCrystal_I2C.h b/LiquidCrystal_I2C.h
index 5d5a741..daecd61 100644
--- a/LiquidCrystal_I2C.h
+++ b/LiquidCrystal_I2C.h
@@ -62,33 +62,38 @@
 #define Rw (1 << BACKPACK_Rw_BIT)  // Read/Write bit
 #define Rs (1 << BACKPACK_Rs_BIT)  // Register select bit
 
-class LiquidCrystal_I2C : public Display {
+class LiquidCrystal_I2C : public DisplayDevice {
 public:
   LiquidCrystal_I2C(I2CAddress lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows);
-  void begin();
+  bool begin() override;
   void clearNative() override;
   void setRowNative(byte line) override;
   size_t writeNative(uint8_t c) override;
+  // I/O is synchronous, so if this is called we're not busy!
+  bool isBusy() override; 
   
   void display();
   void noBacklight();
   void backlight();
   
   void command(uint8_t);
+  uint16_t getNumCols() { return lcdCols; }
+  uint16_t getNumRows() { return lcdRows; }
+
 
 private:
   void send(uint8_t, uint8_t);
   void write4bits(uint8_t);
   void expanderWrite(uint8_t);
-  uint8_t _Addr;
+  uint8_t lcdCols=0, lcdRows=0;
+  I2CAddress _Addr;
   uint8_t _displayfunction;
   uint8_t _displaycontrol;
   uint8_t _displaymode;
   uint8_t _backlightval;
 
   uint8_t outputBuffer[4];
-  // I/O is synchronous, so if this is called we're not busy!
-  bool isBusy() override { return false; } 
+  I2CRB rb;
 };
 
 #endif
diff --git a/SSD1306Ascii.cpp b/SSD1306Ascii.cpp
index 5e24c7a..f5dd281 100644
--- a/SSD1306Ascii.cpp
+++ b/SSD1306Ascii.cpp
@@ -144,39 +144,38 @@ const uint8_t FLASH SSD1306AsciiWire::SH1106_132x64init[] = {
 //------------------------------------------------------------------------------
  
 // Constructor
-SSD1306AsciiWire::SSD1306AsciiWire() {
-  I2CManager.begin();
-  I2CManager.setClock(400000L);   // Set max supported I2C speed
-
+SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
+  m_i2cAddr = 0;
+  m_displayWidth = width;
+  m_displayHeight = height;
 }
 
 // CS auto-detect and configure constructor
-SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
-  I2CManager.begin();
-  I2CManager.setClock(400000L);  // Set max supported I2C speed
-
-  // Probe for I2C device on 0x3c and 0x3d.
-  for (uint8_t address = 0x3c; address <= 0x3d; address++) {
-    if (I2CManager.exists(address)) {
-      begin(address, width, height);
-      // Set singleton Address so CS is able to call it.
-      displayHandler = this;
-      return;
-    }
-  }
-  DIAG(F("OLED display not found"));
-}
-
-bool SSD1306AsciiWire::begin(I2CAddress address, int width, int height) {
-  if (m_initialised) return true;
-
+SSD1306AsciiWire::SSD1306AsciiWire(I2CAddress address, int width, int height) {
   m_i2cAddr = address;
   m_displayWidth = width;
   m_displayHeight = height;
-  
-  // Set size in characters in base class
-  lcdRows = height / 8;
-  lcdCols = width / 6;
+}
+
+bool SSD1306AsciiWire::begin() {
+  I2CManager.begin();
+  I2CManager.setClock(400000L);  // Set max supported I2C speede
+
+  if (m_i2cAddr == 0) {
+    // Probe for I2C device on 0x3c and 0x3d.
+    for (uint8_t address = 0x3c; address <= 0x3d; address++) {
+      if (I2CManager.exists(address)) {
+        m_i2cAddr = address;
+        break;
+      }
+    }
+    if (m_i2cAddr == 0)
+      DIAG(F("OLED display not found"));
+  }
+
+  // Set size in characters
+  m_charsPerColumn = m_displayHeight / fontHeight;
+  m_charsPerRow = (m_displayWidth+fontWidth-1) / fontWidth; // Round up
   m_col = 0;
   m_row = 0;
   m_colOffset = 0;
@@ -186,7 +185,7 @@ bool SSD1306AsciiWire::begin(I2CAddress address, int width, int height) {
     m_colOffset = 2;
     I2CManager.write_P(m_i2cAddr, SH1106_132x64init, sizeof(SH1106_132x64init));
   } else if (m_displayWidth==128 && (m_displayHeight==64 || m_displayHeight==32)) {
-    // SSD1306 128x64 or 128x32
+    // SSD1306 or SSD1309 128x64 or 128x32
     I2CManager.write_P(m_i2cAddr, Adafruit128xXXinit, sizeof(Adafruit128xXXinit));
     if (m_displayHeight == 32) 
       I2CManager.write(m_i2cAddr, 5, 0, // Set command mode
@@ -198,19 +197,18 @@ bool SSD1306AsciiWire::begin(I2CAddress address, int width, int height) {
   }
   // Device found
   DIAG(F("%dx%d OLED display configured on I2C:%s"), m_displayWidth, m_displayHeight, m_i2cAddr.toString());
-  clear();
   return true;
 }
 
 /* Clear screen by writing blank pixels. */
 void SSD1306AsciiWire::clearNative() {
-  const int maxBytes = sizeof(blankPixels);  // max number of bytes sendable over Wire
+  const int maxBytes = sizeof(blankPixels) - 1;  // max number of pixel columns (bytes) per transmission
   for (uint8_t r = 0; r <= m_displayHeight/8 - 1; r++) {
     setRowNative(r);   // Position at start of row to be erased
-    for (uint8_t c = 0; c <= m_displayWidth - 1; c += maxBytes-1) {
-      uint8_t len = m_displayWidth-c+1;
+    for (uint8_t c = 0; c < m_displayWidth; c += maxBytes) {
+      uint8_t len = m_displayWidth-c;  // Number of pixel columns remaining
       if (len > maxBytes) len = maxBytes;
-      I2CManager.write_P(m_i2cAddr, blankPixels, len);  // Write a number of blank columns
+      I2CManager.write_P(m_i2cAddr, blankPixels, len+1);  // Write command + 'len' blank columns
     }
   }
 }
@@ -263,127 +261,262 @@ size_t SSD1306AsciiWire::writeNative(uint8_t ch) {
   outputBuffer[0] = 0x40;     // set SSD1306 controller to data mode
   uint8_t bufferPos = 1;
   // Copy character pixel columns
-  for (uint8_t i = 0; i < fontWidth; i++) 
-    outputBuffer[bufferPos++] = GETFLASH(base++);
-  // Add blank pixels between letters
-  for (uint8_t i = 0; i < letterSpacing; i++) 
-    outputBuffer[bufferPos++] = 0;
+  for (uint8_t i = 0; i < fontWidth; i++) {
+    if (m_col++ < m_displayWidth) 
+      outputBuffer[bufferPos++] = GETFLASH(base++);
+  }
 
   // Write the data to I2C display
   I2CManager.write(m_i2cAddr, outputBuffer, bufferPos, &requestBlock);
-  m_col += fontWidth + letterSpacing;
   return 1;
 }
 
 
 //------------------------------------------------------------------------------
 
-// Font characters, 5x7 pixels, 0x61 characters starting at 0x20.
+// Font characters, 6x8 pixels, starting at 0x20.
 // Lower case characters optionally omitted.
-const uint8_t FLASH SSD1306AsciiWire::System5x7[] = {
+const uint8_t FLASH SSD1306AsciiWire::System6x8[] = {
 
     // Fixed width; char width table not used !!!!
-    // or with lowercase character omitted.
 
     // font data
-    0x00, 0x00, 0x00, 0x00, 0x00,  // (space)
-    0x00, 0x00, 0x5F, 0x00, 0x00,  // !
-    0x00, 0x07, 0x00, 0x07, 0x00,  // "
-    0x14, 0x7F, 0x14, 0x7F, 0x14,  // #
-    0x24, 0x2A, 0x7F, 0x2A, 0x12,  // $
-    0x23, 0x13, 0x08, 0x64, 0x62,  // %
-    0x36, 0x49, 0x55, 0x22, 0x50,  // &
-    0x00, 0x05, 0x03, 0x00, 0x00,  // '
-    0x00, 0x1C, 0x22, 0x41, 0x00,  // (
-    0x00, 0x41, 0x22, 0x1C, 0x00,  // )
-    0x08, 0x2A, 0x1C, 0x2A, 0x08,  // *
-    0x08, 0x08, 0x3E, 0x08, 0x08,  // +
-    0x00, 0x50, 0x30, 0x00, 0x00,  // ,
-    0x08, 0x08, 0x08, 0x08, 0x08,  // -
-    0x00, 0x60, 0x60, 0x00, 0x00,  // .
-    0x20, 0x10, 0x08, 0x04, 0x02,  // /
-    0x3E, 0x51, 0x49, 0x45, 0x3E,  // 0
-    0x00, 0x42, 0x7F, 0x40, 0x00,  // 1
-    0x42, 0x61, 0x51, 0x49, 0x46,  // 2
-    0x21, 0x41, 0x45, 0x4B, 0x31,  // 3
-    0x18, 0x14, 0x12, 0x7F, 0x10,  // 4
-    0x27, 0x45, 0x45, 0x45, 0x39,  // 5
-    0x3C, 0x4A, 0x49, 0x49, 0x30,  // 6
-    0x01, 0x71, 0x09, 0x05, 0x03,  // 7
-    0x36, 0x49, 0x49, 0x49, 0x36,  // 8
-    0x06, 0x49, 0x49, 0x29, 0x1E,  // 9
-    0x00, 0x36, 0x36, 0x00, 0x00,  // :
-    0x00, 0x56, 0x36, 0x00, 0x00,  // ;
-    0x00, 0x08, 0x14, 0x22, 0x41,  // <
-    0x14, 0x14, 0x14, 0x14, 0x14,  // =
-    0x41, 0x22, 0x14, 0x08, 0x00,  // >
-    0x02, 0x01, 0x51, 0x09, 0x06,  // ?
-    0x32, 0x49, 0x79, 0x41, 0x3E,  // @
-    0x7E, 0x11, 0x11, 0x11, 0x7E,  // A
-    0x7F, 0x49, 0x49, 0x49, 0x36,  // B
-    0x3E, 0x41, 0x41, 0x41, 0x22,  // C
-    0x7F, 0x41, 0x41, 0x22, 0x1C,  // D
-    0x7F, 0x49, 0x49, 0x49, 0x41,  // E
-    0x7F, 0x09, 0x09, 0x01, 0x01,  // F
-    0x3E, 0x41, 0x41, 0x51, 0x32,  // G
-    0x7F, 0x08, 0x08, 0x08, 0x7F,  // H
-    0x00, 0x41, 0x7F, 0x41, 0x00,  // I
-    0x20, 0x40, 0x41, 0x3F, 0x01,  // J
-    0x7F, 0x08, 0x14, 0x22, 0x41,  // K
-    0x7F, 0x40, 0x40, 0x40, 0x40,  // L
-    0x7F, 0x02, 0x04, 0x02, 0x7F,  // M
-    0x7F, 0x04, 0x08, 0x10, 0x7F,  // N
-    0x3E, 0x41, 0x41, 0x41, 0x3E,  // O
-    0x7F, 0x09, 0x09, 0x09, 0x06,  // P
-    0x3E, 0x41, 0x51, 0x21, 0x5E,  // Q
-    0x7F, 0x09, 0x19, 0x29, 0x46,  // R
-    0x46, 0x49, 0x49, 0x49, 0x31,  // S
-    0x01, 0x01, 0x7F, 0x01, 0x01,  // T
-    0x3F, 0x40, 0x40, 0x40, 0x3F,  // U
-    0x1F, 0x20, 0x40, 0x20, 0x1F,  // V
-    0x7F, 0x20, 0x18, 0x20, 0x7F,  // W
-    0x63, 0x14, 0x08, 0x14, 0x63,  // X
-    0x03, 0x04, 0x78, 0x04, 0x03,  // Y
-    0x61, 0x51, 0x49, 0x45, 0x43,  // Z
-    0x00, 0x00, 0x7F, 0x41, 0x41,  // [
-    0x02, 0x04, 0x08, 0x10, 0x20,  // "\"
-    0x41, 0x41, 0x7F, 0x00, 0x00,  // ]
-    0x04, 0x02, 0x01, 0x02, 0x04,  // ^
-    0x40, 0x40, 0x40, 0x40, 0x40,  // _
-    0x00, 0x01, 0x02, 0x04, 0x00,  // `
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // (space) (20)
+    0x00, 0x00, 0x5F, 0x00, 0x00, 0x00,  // ! (21)
+    0x00, 0x07, 0x00, 0x07, 0x00, 0x00,  // "
+    0x14, 0x7F, 0x14, 0x7F, 0x14, 0x00,  // #
+    0x24, 0x2A, 0x7F, 0x2A, 0x12, 0x00,  // $
+    0x23, 0x13, 0x08, 0x64, 0x62, 0x00,  // %
+    0x36, 0x49, 0x55, 0x22, 0x50, 0x00,  // &
+    0x00, 0x05, 0x03, 0x00, 0x00, 0x00,  // '
+    0x00, 0x1C, 0x22, 0x41, 0x00, 0x00,  // (
+    0x00, 0x41, 0x22, 0x1C, 0x00, 0x00,  // )
+    0x08, 0x2A, 0x1C, 0x2A, 0x08, 0x00,  // *
+    0x08, 0x08, 0x3E, 0x08, 0x08, 0x00,  // +
+    0x00, 0x50, 0x30, 0x00, 0x00, 0x00,  // ,
+    0x08, 0x08, 0x08, 0x08, 0x08, 0x00,  // -
+    0x00, 0x60, 0x60, 0x00, 0x00, 0x00,  // .
+    0x20, 0x10, 0x08, 0x04, 0x02, 0x00,  // / (47)
+    0x3E, 0x51, 0x49, 0x45, 0x3E, 0x00,  // 0 (48)
+    0x00, 0x42, 0x7F, 0x40, 0x00, 0x00,  // 1
+    0x42, 0x61, 0x51, 0x49, 0x46, 0x00,  // 2
+    0x21, 0x41, 0x45, 0x4B, 0x31, 0x00,  // 3
+    0x18, 0x14, 0x12, 0x7F, 0x10, 0x00,  // 4
+    0x27, 0x45, 0x45, 0x45, 0x39, 0x00,  // 5
+    0x3C, 0x4A, 0x49, 0x49, 0x30, 0x00,  // 6
+    0x01, 0x71, 0x09, 0x05, 0x03, 0x00,  // 7
+    0x36, 0x49, 0x49, 0x49, 0x36, 0x00,  // 8
+    0x06, 0x49, 0x49, 0x29, 0x1E, 0x00,  // 9 (57)
+    0x00, 0x36, 0x36, 0x00, 0x00, 0x00,  // :
+    0x00, 0x56, 0x36, 0x00, 0x00, 0x00,  // ;
+    0x00, 0x08, 0x14, 0x22, 0x41, 0x00,  // <
+    0x14, 0x14, 0x14, 0x14, 0x14, 0x00,  // =
+    0x41, 0x22, 0x14, 0x08, 0x00, 0x00,  // >
+    0x02, 0x01, 0x51, 0x09, 0x06, 0x00,  // ?
+    0x32, 0x49, 0x79, 0x41, 0x3E, 0x00,  // @ (64)
+    0x7E, 0x11, 0x11, 0x11, 0x7E, 0x00,  // A (65)
+    0x7F, 0x49, 0x49, 0x49, 0x36, 0x00,  // B
+    0x3E, 0x41, 0x41, 0x41, 0x22, 0x00,  // C
+    0x7F, 0x41, 0x41, 0x22, 0x1C, 0x00,  // D
+    0x7F, 0x49, 0x49, 0x49, 0x41, 0x00,  // E
+    0x7F, 0x09, 0x09, 0x01, 0x01, 0x00,  // F
+    0x3E, 0x41, 0x41, 0x51, 0x32, 0x00,  // G
+    0x7F, 0x08, 0x08, 0x08, 0x7F, 0x00,  // H
+    0x00, 0x41, 0x7F, 0x41, 0x00, 0x00,  // I
+    0x20, 0x40, 0x41, 0x3F, 0x01, 0x00,  // J
+    0x7F, 0x08, 0x14, 0x22, 0x41, 0x00,  // K
+    0x7F, 0x40, 0x40, 0x40, 0x40, 0x00,  // L
+    0x7F, 0x02, 0x04, 0x02, 0x7F, 0x00,  // M
+    0x7F, 0x04, 0x08, 0x10, 0x7F, 0x00,  // N
+    0x3E, 0x41, 0x41, 0x41, 0x3E, 0x00,  // O
+    0x7F, 0x09, 0x09, 0x09, 0x06, 0x00,  // P
+    0x3E, 0x41, 0x51, 0x21, 0x5E, 0x00,  // Q
+    0x7F, 0x09, 0x19, 0x29, 0x46, 0x00,  // R
+    0x46, 0x49, 0x49, 0x49, 0x31, 0x00,  // S
+    0x01, 0x01, 0x7F, 0x01, 0x01, 0x00,  // T
+    0x3F, 0x40, 0x40, 0x40, 0x3F, 0x00,  // U
+    0x1F, 0x20, 0x40, 0x20, 0x1F, 0x00,  // V
+    0x7F, 0x20, 0x18, 0x20, 0x7F, 0x00,  // W
+    0x63, 0x14, 0x08, 0x14, 0x63, 0x00,  // X
+    0x03, 0x04, 0x78, 0x04, 0x03, 0x00,  // Y
+    0x61, 0x51, 0x49, 0x45, 0x43, 0x00,  // Z (90)
+    0x00, 0x00, 0x7F, 0x41, 0x41, 0x00,  // [
+    0x02, 0x04, 0x08, 0x10, 0x20, 0x00,  // "\"
+    0x41, 0x41, 0x7F, 0x00, 0x00, 0x00,  // ]
+    0x04, 0x02, 0x01, 0x02, 0x04, 0x00,  // ^
+    0x40, 0x40, 0x40, 0x40, 0x40, 0x00,  // _
+    0x00, 0x01, 0x02, 0x04, 0x00, 0x00,  // ' (96)
 #ifndef NOLOWERCASE
-    0x20, 0x54, 0x54, 0x54, 0x78,  // a
-    0x7F, 0x48, 0x44, 0x44, 0x38,  // b
-    0x38, 0x44, 0x44, 0x44, 0x20,  // c
-    0x38, 0x44, 0x44, 0x48, 0x7F,  // d
-    0x38, 0x54, 0x54, 0x54, 0x18,  // e
-    0x08, 0x7E, 0x09, 0x01, 0x02,  // f
-    0x08, 0x14, 0x54, 0x54, 0x3C,  // g
-    0x7F, 0x08, 0x04, 0x04, 0x78,  // h
-    0x00, 0x44, 0x7D, 0x40, 0x00,  // i
-    0x20, 0x40, 0x44, 0x3D, 0x00,  // j
-    0x00, 0x7F, 0x10, 0x28, 0x44,  // k
-    0x00, 0x41, 0x7F, 0x40, 0x00,  // l
-    0x7C, 0x04, 0x18, 0x04, 0x78,  // m
-    0x7C, 0x08, 0x04, 0x04, 0x78,  // n
-    0x38, 0x44, 0x44, 0x44, 0x38,  // o
-    0x7C, 0x14, 0x14, 0x14, 0x08,  // p
-    0x08, 0x14, 0x14, 0x18, 0x7C,  // q
-    0x7C, 0x08, 0x04, 0x04, 0x08,  // r
-    0x48, 0x54, 0x54, 0x54, 0x20,  // s
-    0x04, 0x3F, 0x44, 0x40, 0x20,  // t
-    0x3C, 0x40, 0x40, 0x20, 0x7C,  // u
-    0x1C, 0x20, 0x40, 0x20, 0x1C,  // v
-    0x3C, 0x40, 0x30, 0x40, 0x3C,  // w
-    0x44, 0x28, 0x10, 0x28, 0x44,  // x
-    0x0C, 0x50, 0x50, 0x50, 0x3C,  // y
-    0x44, 0x64, 0x54, 0x4C, 0x44,  // z
+    0x20, 0x54, 0x54, 0x54, 0x78, 0x00,  // a (97)
+    0x7F, 0x48, 0x44, 0x44, 0x38, 0x00,  // b
+    0x38, 0x44, 0x44, 0x44, 0x20, 0x00,  // c
+    0x38, 0x44, 0x44, 0x48, 0x7F, 0x00,  // d
+    0x38, 0x54, 0x54, 0x54, 0x18, 0x00,  // e
+    0x08, 0x7E, 0x09, 0x01, 0x02, 0x00,  // f
+    0x08, 0x14, 0x54, 0x54, 0x3C, 0x00,  // g
+    0x7F, 0x08, 0x04, 0x04, 0x78, 0x00,  // h
+    0x00, 0x44, 0x7D, 0x40, 0x00, 0x00,  // i
+    0x20, 0x40, 0x44, 0x3D, 0x00, 0x00,  // j
+    0x00, 0x7F, 0x10, 0x28, 0x44, 0x00,  // k
+    0x00, 0x41, 0x7F, 0x40, 0x00, 0x00,  // l
+    0x7C, 0x04, 0x18, 0x04, 0x78, 0x00,  // m
+    0x7C, 0x08, 0x04, 0x04, 0x78, 0x00,  // n
+    0x38, 0x44, 0x44, 0x44, 0x38, 0x00,  // o
+    0x7C, 0x14, 0x14, 0x14, 0x08, 0x00,  // p
+    0x08, 0x14, 0x14, 0x18, 0x7C, 0x00,  // q
+    0x7C, 0x08, 0x04, 0x04, 0x08, 0x00,  // r
+    0x48, 0x54, 0x54, 0x54, 0x20, 0x00,  // s
+    0x04, 0x3F, 0x44, 0x40, 0x20, 0x00,  // t
+    0x3C, 0x40, 0x40, 0x20, 0x7C, 0x00,  // u
+    0x1C, 0x20, 0x40, 0x20, 0x1C, 0x00,  // v
+    0x3C, 0x40, 0x30, 0x40, 0x3C, 0x00,  // w
+    0x44, 0x28, 0x10, 0x28, 0x44, 0x00,  // x
+    0x0C, 0x50, 0x50, 0x50, 0x3C, 0x00,  // y
+    0x44, 0x64, 0x54, 0x4C, 0x44, 0x00,  // z (122)
 #endif
-    0x00, 0x08, 0x36, 0x41, 0x00,  // {
-    0x00, 0x00, 0x7F, 0x00, 0x00,  // |
-    0x00, 0x41, 0x36, 0x08, 0x00,  // }
-    0x08, 0x08, 0x2A, 0x1C, 0x08,  // ->
-    0x08, 0x1C, 0x2A, 0x08, 0x08,  // <-
-    0x00, 0x06, 0x09, 0x09, 0x06   // degree symbol
+    0x00, 0x08, 0x36, 0x41, 0x00, 0x00,  // { (123)
+    0x00, 0x00, 0x7F, 0x00, 0x00, 0x00,  // |
+    0x00, 0x41, 0x36, 0x08, 0x00, 0x00,  // }
+    0x08, 0x08, 0x2A, 0x1C, 0x08, 0x00,  // ->
+    0x08, 0x1C, 0x2A, 0x08, 0x08, 0x00,  // <- (127)
+#ifndef NO_EXTENDED_CHARACTERS
+// Extended characters - based on "DOS Western Europe" characters
+//   International characters not yet implemented.
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0x80
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0x90
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0xa0
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    // Extended characters 176-180
+    0x92, 0x00, 0x49, 0x00, 0x24, 0x00,  // Light grey 0xb0
+    0xcc, 0x55, 0xcc, 0x55, 0xcc, 0x55,  // Mid grey 0xb1
+    0x6a, 0xff, 0xb6, 0xff, 0xdb, 0xff,  // Dark grey 0xb2
+    0x00, 0x00, 0x00, 0xff, 0x00, 0x00,  // Vertical line 0xb3
+    0x08, 0x08, 0x08, 0xff, 0x00, 0x00,  // Vertical line with left spur 0xb4
 
+    0x14, 0x14, 0xfe, 0x00, 0xff, 0x00,  // Vertical line with double left spur 0xb9
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented Double vertical line with single left spur
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+
+    // Extended characters 185-190
+    0x28, 0x28, 0xef, 0x00, 0xff, 0x00,  // Double vertical line with double left spur 0xb9
+    0x00, 0x00, 0xff, 0x00, 0xff, 0x00,  // Double vertical line 0xba
+    0x14, 0x14, 0xf4, 0x04, 0xfc, 0x00,  // Double top right corner 0xbb
+    0x14, 0x14, 0x17, 0x10, 0x1f, 0x00,  // Double bottom right corner 0xbc
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0xbd
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0xbe
+
+    // Extended characters 191-199
+    0x08, 0x08, 0x08, 0xf8, 0x00, 0x00,  // Top right corner 0xbf
+    0x00, 0x00, 0x00, 0x0f, 0x08, 0x08,  // Bottom left corner 0xc0
+    0x08, 0x08, 0x08, 0x0f, 0x08, 0x08,  // Horizontal line with upward spur 0xc1
+    0x08, 0x08, 0x08, 0xf8, 0x08, 0x08,  // Horizontal line with downward spur 0xc2
+    0x00, 0x00, 0x00, 0xff, 0x08, 0x08,  // Vertical line with right spur 0xc3
+    0x08, 0x08, 0x08, 0x08, 0x08, 0x08,  // Horizontal line 0xc4
+    0x08, 0x08, 0x08, 0xff, 0x08, 0x08,  // Cross 0xc5
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+
+    // Extended characters 200-206
+    0x00, 0x00, 0x1f, 0x10, 0x17, 0x14,  // Double bottom left corner 0xc8
+    0x00, 0x00, 0xfc, 0x04, 0xf4, 0x14,  // Double top left corner 0xc9
+    0x14, 0x14, 0x17, 0x10, 0x17, 0x14,  // Double horizontal with double upward spur 0xca
+    0x14, 0x14, 0xf4, 0x04, 0xf4, 0x14,  // Double horizontal with double downward spur 0xcb
+    0x00, 0x00, 0xff, 0x00, 0xf7, 0x14,  // Double vertical line with double right spur 0xcc
+    0x14, 0x14, 0x14, 0x14, 0x14, 0x14,  // Double horizontal line 0xcd
+    0x14, 0x14, 0xf7, 0x00, 0xf7, 0x14,  // Double cross 0xce
+
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0xd0
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+
+    // Extended characters 217-223
+    0x08, 0x08, 0x08, 0x0f, 0x00, 0x00,  // Bottom right corner 0xd9
+    0x00, 0x00, 0x00, 0xf8, 0x08, 0x08,  // Top left corner 0xda
+    0xff, 0xff, 0xff, 0xff, 0xff, 0xff,  // Solid block 0xdb
+    0xf0, 0xf0, 0xf0, 0xf0, 0xf0, 0xf0,  // Bottom half block 0xdc
+    0xff, 0xff, 0xff, 0x00, 0x00, 0x00,  // Left half block 0xdd
+    0x00, 0x00, 0x00, 0xff, 0xff, 0xff,  // Right half block 0xde
+    0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f,  // Top half block 0xdf
+
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0xe0
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented 0xf0
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  // Not implemented
+    // Extended character 248
+    0x00, 0x06, 0x09, 0x09, 0x06, 0x00,  // degree symbol 0xf8
+#endif
+    0x00
 };
+
+const uint8_t SSD1306AsciiWire::m_fontCharCount = sizeof(System6x8) / 6;
diff --git a/SSD1306Ascii.h b/SSD1306Ascii.h
index d40eac6..57427a1 100644
--- a/SSD1306Ascii.h
+++ b/SSD1306Ascii.h
@@ -27,21 +27,24 @@
 
 #include "I2CManager.h"
 #include "DIAG.h"
+#include "DisplayInterface.h"
 
 // Uncomment to remove lower-case letters to save 108 bytes of flash
 //#define NOLOWERCASE
 
+
+
 //------------------------------------------------------------------------------
 // Constructor
-class SSD1306AsciiWire : public Display {
+class SSD1306AsciiWire : public DisplayDevice {
  public:
 
   // Constructors
   SSD1306AsciiWire(int width, int height); // Auto-detects I2C address
-  SSD1306AsciiWire();  // Requires call to 'begin()'
+  SSD1306AsciiWire(I2CAddress address, int width, int height);  
 
   // Initialize the display controller.
-  bool begin(I2CAddress address, int width, int height);
+  bool begin();
 
   // Clear the display and set the cursor to (0, 0).
   void clearNative() override;
@@ -53,6 +56,8 @@ class SSD1306AsciiWire : public Display {
   size_t writeNative(uint8_t c) override;
 
   bool isBusy() override { return requestBlock.isBusy(); }
+  uint16_t getNumCols() { return m_charsPerRow; }
+  uint16_t getNumRows() { return m_charsPerColumn; }
 
  private:
   // Cursor column.
@@ -63,28 +68,31 @@ class SSD1306AsciiWire : public Display {
   uint8_t m_displayWidth;
   // Display height.
   uint8_t m_displayHeight;
+  // Display width in characters
+  uint8_t m_charsPerRow;
+  // Display height in characters
+  uint8_t m_charsPerColumn;
   // Column offset RAM to SEG.
   uint8_t m_colOffset = 0;
   // Current font.
-  const uint8_t* const m_font = System5x7;
+  const uint8_t* const m_font = System6x8;
   // Flag to prevent calling begin() twice
   uint8_t m_initialised = false;
 
-  // Only fixed size 5x7 fonts in a 6x8 cell are supported.
-  static const uint8_t fontWidth = 5;
-  static const uint8_t fontHeight = 7;
-  static const uint8_t letterSpacing = 1;
+  // Only fixed size 6x8 fonts in a 6x8 cell are supported.
+  static const uint8_t fontWidth = 6;
+  static const uint8_t fontHeight = 8;
   static const uint8_t m_fontFirstChar = 0x20;
-  static const uint8_t m_fontCharCount = 0x61;
+  static const uint8_t m_fontCharCount;
 
-  I2CAddress m_i2cAddr;
+  I2CAddress m_i2cAddr = 0;
 
   I2CRB requestBlock;
-  uint8_t outputBuffer[fontWidth+letterSpacing+1];
+  uint8_t outputBuffer[fontWidth+1];
 
   static const uint8_t blankPixels[];
 
-  static const uint8_t System5x7[];
+  static const uint8_t System6x8[];
   static const uint8_t FLASH Adafruit128xXXinit[];
   static const uint8_t FLASH SH1106_132x64init[];
 };
diff --git a/StringFormatter.cpp b/StringFormatter.cpp
index a659fc4..f7d9c50 100644
--- a/StringFormatter.cpp
+++ b/StringFormatter.cpp
@@ -18,7 +18,7 @@
  */
 #include "StringFormatter.h"
 #include <stdarg.h>
-#include "Display.h"
+#include "DisplayInterface.h"
 
 bool Diag::ACK=false;
 bool Diag::CMD=false;
@@ -45,19 +45,17 @@ void StringFormatter::lcd(byte row, const FSH* input...) {
   send2(&USB_SERIAL,input,args);
   send(&USB_SERIAL,F(" *>\n"));
   
-  if (!Display::displayHandler) return;
-  Display::displayHandler->setRow(row);    
+  DisplayInterface::setRow(row);    
   va_start(args, input);
-  send2(Display::displayHandler,input,args);
+  send2(DisplayInterface::getDisplayHandler(),input,args);
 }
 
 void StringFormatter::lcd2(uint8_t display, byte row, const FSH* input...) {
   va_list args;
 
-  if (!Display::displayHandler) return;
-  Display::displayHandler->setRow(display, row);    
+  DisplayInterface::setRow(display, row);    
   va_start(args, input);
-  send2(Display::displayHandler,input,args);
+  send2(DisplayInterface::getDisplayHandler(),input,args);
 }
 
 void StringFormatter::send(Print * stream, const FSH* input...) {
diff --git a/defines.h b/defines.h
index e9ae631..9748824 100644
--- a/defines.h
+++ b/defines.h
@@ -46,9 +46,13 @@
 #if defined(ARDUINO_AVR_UNO)
 #define ARDUINO_TYPE "UNO"
 #undef HAS_ENOUGH_MEMORY
+#define NO_EXTENDED_CHARACTERS
+#undef I2C_EXTENDED_ADDRESS
 #elif defined(ARDUINO_AVR_NANO)
 #define ARDUINO_TYPE "NANO"
 #undef HAS_ENOUGH_MEMORY
+#define NO_EXTENDED_CHARACTERS
+#undef I2C_EXTENDED_ADDRESS
 #elif defined(ARDUINO_AVR_MEGA)
 #define ARDUINO_TYPE "MEGA"
 #elif defined(ARDUINO_AVR_MEGA2560)