Update MultiLCD library

7 files changed, 546 insertions, 327 deletions

diff --git a/libraries/MultiLCD/ILI9325D.cpp b/libraries/MultiLCD/ILI9325D.cpp
index f977d94..ee774d4 100644
--- a/libraries/MultiLCD/ILI9325D.cpp
+++ b/libraries/MultiLCD/ILI9325D.cpp
@@ -412,10 +412,10 @@ void LCD_ILI9325D::writeDigit(byte n)
     }
 }
 
-void LCD_ILI9325D::draw(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint16_t width, uint16_t height)
+void LCD_ILI9325D::draw(const PROGMEM byte* buffer, uint16_t width, uint16_t height)
 {
     byte rows = height >> 3;
-    setXY(y, y + height - 1, x, x + width - 1);
+    setXY(m_y, m_y + height - 1, m_x, m_x + width - 1);
     uint16_t i = width - 1;
     do {
         for (uint8_t h = 0; h < rows; h++) {
@@ -425,13 +425,14 @@ void LCD_ILI9325D::draw(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint
             }
         }
     } while (i--);
+    m_x += width;
 }
 
-void LCD_ILI9325D::draw2x(const PROGMEM byte* buffer, uint16_t x, uint16_t y, byte width, byte height)
+void LCD_ILI9325D::draw2x(const PROGMEM byte* buffer, byte width, byte height)
 {
     char buf[240];
     uint16_t pixels = (uint16_t)width * height;
-    setXY(y, y + height * 2 - 1, x, x + width * 2- 1);
+    setXY(m_y, m_y + height * 2 - 1, m_x, m_x + width * 2- 1);
     uint16_t i = width - 1;
     do {
         memcpy_P(buf, buffer + (uint16_t)i * height * 2, height * 2);
@@ -444,6 +445,7 @@ void LCD_ILI9325D::draw2x(const PROGMEM byte* buffer, uint16_t x, uint16_t y, by
             WriteData(buf[j], buf[j + 1]);
         }
     } while (i--);
+    m_x += width * 2;
 }
 
 void LCD_ILI9325D::draw4bpp(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint16_t width, uint16_t height)
diff --git a/libraries/MultiLCD/ILI9341.cpp b/libraries/MultiLCD/ILI9341.cpp
index b09d3a6..ea569e8 100644
--- a/libraries/MultiLCD/ILI9341.cpp
+++ b/libraries/MultiLCD/ILI9341.cpp
@@ -358,8 +358,8 @@ void LCD_ILI9341::clear(void)
 
 void LCD_ILI9341::setXY(uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
 {
-    setCol(x0, x1);
-    setPage(319 - y1, 319 - y0);
+    setCol(239 - x1, 239 - x0);
+    setPage(y0, y1);
     sendCMD(0x2c);
 }
 
@@ -382,20 +382,31 @@ void LCD_ILI9341::clearPixels(uint16_t pixels)
     {
         SPI.transfer(0);
         SPI.transfer(0);
-        SPI.transfer(0);
-        SPI.transfer(0);
     }
     TFT_CS_HIGH;
 }
 
+void LCD_ILI9341::sendPixelData(byte d)
+{
+    for (byte j = 0; j < 8; j++, d <<= 1) {
+        if (d & 0x80) {
+            SPI.transfer(m_color[1][1]);
+            SPI.transfer(m_color[1][0]);
+        } else {
+            SPI.transfer(m_color[0][1]);
+            SPI.transfer(m_color[0][0]);
+        }
+    }
+}
+
 size_t LCD_ILI9341::write(uint8_t c)
 {
     if (c == '\n') {
-        m_x += (m_font + 1) << 3;
+        m_y += (m_font + 1) << 3;
         return 0;
     } else if (c == '\r') {
-        setXY(m_x, m_x + 7, m_y, 319);
-        clearPixels((320 - m_y) * 8);
+        setXY(m_y, m_y + 7, m_x, 319);
+        clearPixels((320 - m_x) * 8);
         m_y = 0;
         return 0;
     }
@@ -403,42 +414,33 @@ size_t LCD_ILI9341::write(uint8_t c)
 #ifndef MEMORY_SAVING
     if (m_font == FONT_SIZE_SMALL) {
 #endif
-        setXY(m_x, m_x + 7, m_y, m_y + 4);
-        m_y += 6;
-        if (m_y >= 320) {
-            m_x += (m_font + 1) << 3;
-            m_y = 0;
-            if (m_x >= 240) {
-                m_x = 0;
-            }
+        setXY(m_y, m_y + 7, m_x, m_x + 4);
+        m_x += 6;
+        if (m_x >= 320) {
+            m_y += 8;
+            m_x = 0;
+            if (m_y >= 240) m_y = 0;
         }
         if (c > 0x20 && c < 0x7f) {
             byte pgm_buffer[5];
             memcpy_P(pgm_buffer, &font5x8[c - 0x21], 5);
-            byte i = 4;
             TFT_DC_HIGH;
             TFT_CS_LOW;
-            do {
-                unsigned char d = pgm_buffer[i];
-                for (byte j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
-            } while (i--);
+            for (byte i = 0; i < 5; i++) {
+                sendPixelData(pgm_buffer[i]);
+            }
             TFT_CS_HIGH;
         } else {
             clearPixels(5 * 8);
         }
 #ifndef MEMORY_SAVING
     } else {
-        setXY(m_x, m_x + 15, m_y, m_y + 7);
-        m_y += 9;
-        if (m_y >= 320) {
-            m_x += (m_font + 1) << 3;
-            m_y = 0;
-            if (m_x >= 240) {
-                m_x = 0;
-            }
+        setXY(m_y, m_y + 15, m_x, m_x + 7);
+        m_x += 9;
+        if (m_x >= 320) {
+            m_y += 16;
+            m_x = 0;
+            if (m_y >= 240) m_y = 0;
         }
         if (c > 0x20 && c < 0x7f) {
             byte pgm_buffer[16];
@@ -446,16 +448,8 @@ size_t LCD_ILI9341::write(uint8_t c)
             TFT_DC_HIGH;
             TFT_CS_LOW;
             for (byte i = 0; i < 16; i += 2) {
-                unsigned char d = pgm_buffer[14 - i];
-                for (byte j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
-                d = pgm_buffer[15 - i];
-                for (byte j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
+                sendPixelData(pgm_buffer[i + 1]);
+                sendPixelData(pgm_buffer[i]);
             }
             TFT_CS_HIGH;
         } else {
@@ -467,134 +461,87 @@ size_t LCD_ILI9341::write(uint8_t c)
 
 void LCD_ILI9341::writeDigit(byte n)
 {
-    if (m_font == FONT_SIZE_SMALL) {
-        setXY(m_x, m_x + 7, m_y, m_y + 7);
-        sendCMD(0x2c);
-        if (n <= 9) {
-            byte pgm_buffer[8];
-            memcpy_P(pgm_buffer, &digits8x8[n], 8);
-            byte i = 7;
-            TFT_DC_HIGH;
-            TFT_CS_LOW;
-            do {
-                unsigned char d = pgm_buffer[i];
-                for (byte j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
-            } while (i--);
-            TFT_CS_HIGH;
-            m_y += 8;
-        } else {
-            clearPixels(8 * 8);
-        }
-    } else if (m_font == FONT_SIZE_MEDIUM) {
-        write(n <= 9 ? ('0' + n) : ' ');
-    } else if (m_font == FONT_SIZE_LARGE) {
-        setXY(m_x, m_x + 15, m_y, m_y + 15);
-        m_y += 16;
+    if (m_font == FONT_SIZE_LARGE) {
+        setXY(m_y, m_y + 15, m_x, m_x + 15);
+        m_x += 16;
         if (n <= 9) {
             byte pgm_buffer[32];
             memcpy_P(pgm_buffer, &digits16x16[n], sizeof(pgm_buffer));
             TFT_DC_HIGH;
             TFT_CS_LOW;
             for (byte i = 0; i < 16; i++) {
-                unsigned char d = pgm_buffer[15 - i];
-                for (byte j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
-                d = pgm_buffer[31 - i];
-                for (byte j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
+                sendPixelData(pgm_buffer[16 + i]);
+                sendPixelData(pgm_buffer[i]);
             }
             TFT_CS_HIGH;
         } else {
             clearPixels(16 * 16);
         }
     } else if (m_font == FONT_SIZE_XLARGE) {
-        setXY(m_x, m_x + 23, m_y, m_y + 15);
-        m_y += 18;
+        setXY(m_y, m_y + 23, m_x, m_x + 15);
+        m_x += 17;
         if (n <= 9) {
             byte pgm_buffer[48];
             memcpy_P(pgm_buffer, &digits16x24[n], sizeof(pgm_buffer));
             TFT_DC_HIGH;
             TFT_CS_LOW;
             for (int i = 0; i < 48; i += 3) {
-                unsigned char d = pgm_buffer[45 - i];
-                for (int j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
-                d = pgm_buffer[46 - i];
-                for (int j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
-                d = pgm_buffer[47 - i];
-                for (int j = 0; j < 8; j++, d >>= 1) {
-                    SPI.transfer(m_color[d & 1][1]);
-                    SPI.transfer(m_color[d & 1][0]);
-                }
+                sendPixelData(pgm_buffer[i + 2]);
+                sendPixelData(pgm_buffer[i + 1]);
+                sendPixelData(pgm_buffer[i]);
             }
             TFT_CS_HIGH;
         } else {
             clearPixels(16 * 24);
         }
+    } else {
+        write(n <= 9 ? ('0' + n) : ' ');
     }
 }
 
-void LCD_ILI9341::draw(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint16_t width, uint16_t height)
+void LCD_ILI9341::draw(const PROGMEM byte* buffer, uint16_t width, uint16_t height)
 {
     byte rows = height >> 3;
-    setXY(y, y + height - 1, x, x + width - 1);
+    setXY(m_y, m_y + height - 1, m_x, m_x + width - 1);
     uint16_t i = width - 1;
     TFT_DC_HIGH;
     TFT_CS_LOW;
-    do {
-        for (uint8_t h = 0; h < rows; h++) {
+    for (uint16_t i = 0; i < width; i++) {
+        for (int8_t h = rows - 1; h >= 0; h--) {
             byte d = pgm_read_byte(buffer + i + width * h);
-            for (byte j = 0; j < 8; j++, d >>= 1) {
-                SPI.transfer(m_color[d & 1][1]);
-                SPI.transfer(m_color[d & 1][0]);
-            }
+            sendPixelData(d);
         }
-    } while (i--);
+    }
     TFT_CS_HIGH;
+    m_x += width;
 }
 
-void LCD_ILI9341::draw2x(const PROGMEM byte* buffer, uint16_t x, uint16_t y, byte width, byte height)
+void LCD_ILI9341::draw2x(const PROGMEM byte* buffer, byte width, byte height)
 {
     byte rows = height >> 3;
-    setXY(y, y + height * 2 - 1, x, x + width * 2 - 1);
+    setXY(m_y, m_y + height * 2 - 1, m_x, m_x + width * 2 - 1);
     uint16_t i = width - 1;
+    uint16_t w = width << 1;
     TFT_DC_HIGH;
     TFT_CS_LOW;
-    do {
-        for (uint8_t h = 0; h < rows; h++) {
-            byte d = pgm_read_byte(buffer + i + width * h);
-            for (byte j = 0; j < 8; j++, d >>= 1) {
-                byte h = m_color[d & 1][1];
-                byte l = m_color[d & 1][0];
-                SPI.transfer(h);
-                SPI.transfer(l);
-                SPI.transfer(h);
-                SPI.transfer(l);
-            }
-        }
-        for (uint8_t h = 0; h < rows; h++) {
-            byte d = pgm_read_byte(buffer + i + width * h);
-            for (byte j = 0; j < 8; j++, d >>= 1) {
-                byte h = m_color[d & 1][1];
-                byte l = m_color[d & 1][0];
-                SPI.transfer(h);
-                SPI.transfer(l);
-                SPI.transfer(h);
-                SPI.transfer(l);
+    for (uint16_t i = 0; i < w; i++) {
+        for (int8_t h = rows - 1; h >= 0; h--) {
+            byte d = pgm_read_byte(buffer + (i >> 1) + width * h);
+            for (byte j = 0; j < 8; j++, d <<= 1) {
+                if (d & 0x80) {
+                    SPI.transfer(m_color[1][1]);
+                    SPI.transfer(m_color[1][0]);
+                    SPI.transfer(m_color[1][1]);
+                    SPI.transfer(m_color[1][0]);
+                } else {
+                    SPI.transfer(m_color[0][1]);
+                    SPI.transfer(m_color[0][0]);
+                    SPI.transfer(m_color[0][1]);
+                    SPI.transfer(m_color[0][0]);
+                }
             }
         }
-    } while (i--);
+    };
     TFT_CS_HIGH;
+    m_x += (width << 1);
 }
diff --git a/libraries/MultiLCD/ILI9341.h b/libraries/MultiLCD/ILI9341.h
deleted file mode 100644
index e0075c4..0000000
--- a/libraries/MultiLCD/ILI9341.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/*
- 2012 Copyright (c) Seeed Technology Inc.
-
- Authors: Albert.Miao & Loovee,
- Visweswara R (with initializtion code from TFT vendor)
-
- This library is free software; you can redistribute it and/or
- modify it under the terms of the GNU Lesser General Public
- License as published by the Free Software Foundation; either
- version 2.1 of the License, or (at your option) any later version.
-
- This library is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with this library; if not, write to the Free Software
- Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-
-*/
-#ifndef TFTv2_h
-#define TFTv2_h
-
-#if defined(ARDUINO) && ARDUINO >= 100
-#define SEEEDUINO
-#include <Arduino.h>
-#else
-#include <WProgram.h>
-#endif
-#include <avr/pgmspace.h>
-
-#include <SPI.h>
-
-//Basic Colors
-#define RED		0xf800
-#define GREEN	0x07e0
-#define BLUE	0x001f
-#define BLACK	0x0000
-#define YELLOW	0xffe0
-#define WHITE	0xffff
-
-//Other Colors
-#define CYAN		0x07ff
-#define BRIGHT_RED	0xf810
-#define GRAY1		0x8410
-#define GRAY2		0x4208
-
-//TFT resolution 240*320
-#define MIN_X	0
-#define MIN_Y	0
-#define MAX_X	239
-#define MAX_Y	319
-
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-
-#define TFT_CS_LOW  {DDRE |= 0x08;PORTE &=~ 0x08;}
-#define TFT_CS_HIGH {DDRE |= 0x08;PORTE |=  0x08;}
-#define TFT_DC_LOW  {DDRH |= 0x08;PORTH &=~ 0x08;}
-#define TFT_DC_HIGH {DDRH |= 0x08;PORTH |=  0x08;}
-#define TFT_BL_OFF  {DDRH |= 0x10;PORTH &=~ 0x10;}
-#define TFT_BL_ON   {DDRH |= 0x10;PORTH |=  0x10;}
-#define TFT_RST_OFF {DDRD |= 0x10;PORTD |=  0x10;}
-#define TFT_RST_ON  {DDRD |= 0x10;PORTD &=~ 0x10;}
-
-#define YP A2   // must be an analog pin, use "An" notation!
-#define XM A1   // must be an analog pin, use "An" notation!
-#define YM 54   // can be a digital pin, this is A0
-#define XP 57   // can be a digital pin, this is A3
-
-#elif defined(__AVR_ATmega32U4__)
-
-#define TFT_CS_LOW  {DDRC |= 0x40;PORTC &=~ 0x40;}
-#define TFT_CS_HIGH {DDRC |= 0x40;PORTC |=  0x40;}
-#define TFT_DC_LOW  {DDRD |= 0x80;PORTD &=~ 0x80;}
-#define TFT_DC_HIGH {DDRD |= 0x80;PORTD |=  0x80;}
-#define TFT_BL_OFF  {DDRE |= 0x40;PORTE &=~ 0x40;}
-#define TFT_BL_ON   {DDRE |= 0x40;PORTE |=  0x40;}
-#define TFT_RST_OFF {DDRD |= 0x10;PORTD |=  0x10;}
-#define TFT_RST_ON  {DDRD |= 0x10;PORTD &=~ 0x10;}
-
-#define YP A2   // must be an analog pin, use "An" notation!
-#define XM A1   // must be an analog pin, use "An" notation!
-#define YM 18   // can be a digital pin, this is A0
-#define XP 21   // can be a digital pin, this is A3
-
-#else
-#define TFT_CS_LOW  {DDRD |= 0x20;PORTD &=~ 0x20;}
-#define TFT_CS_HIGH {DDRD |= 0x20;PORTD |=  0x20;}
-#define TFT_DC_LOW  {DDRD |= 0x40;PORTD &=~ 0x40;}
-#define TFT_DC_HIGH {DDRD |= 0x40;PORTD |=  0x40;}
-#define TFT_BL_OFF  {DDRD |= 0x80;PORTD &=~ 0x80;}
-#define TFT_BL_ON   {DDRD |= 0x80;PORTD |=  0x80;}
-#define TFT_RST_OFF {DDRD |= 0x10;PORTD |=  0x10;}
-#define TFT_RST_ON  {DDRD |= 0x10;PORTD &=~ 0x10;}
-
-
-#define YP A2   // must be an analog pin, use "An" notation!
-#define XM A1   // must be an analog pin, use "An" notation!
-#define YM 14   // can be a digital pin, this is A0
-#define XP 17   // can be a digital pin, this is A3
-
-#endif
-
-#define TS_MINX 116*2
-#define TS_MAXX 890*2
-#define TS_MINY 83*2
-#define TS_MAXY 913*2
-
-#ifndef INT8U
-#define INT8U unsigned char
-#endif
-#ifndef INT16U
-#define INT16U unsigned int
-#endif
-
-extern const PROGMEM unsigned char simpleFont[][8];
-
-class TFT
-{
-public:
-	void TFTinit (void);
-	void setCol(INT16U StartCol,INT16U EndCol);
-	void setPage(INT16U StartPage,INT16U EndPage);
-	void setXY(INT16U poX, INT16U poY);
-	void setPixel(INT16U poX, INT16U poY,INT16U color);
-	void sendCMD(INT8U index);
-	void WRITE_Package(INT16U *data,INT8U howmany);
-	void WRITE_DATA(INT8U data);
-	void sendData(INT16U data);
-	INT8U Read_Register(INT8U Addr,INT8U xParameter);
-	void fillScreen(INT16U XL,INT16U XR,INT16U YU,INT16U YD,INT16U color);
-	void fillScreen(void);
-	INT8U readID(void);
-
-	void drawChar(INT8U ascii,INT16U poX, INT16U poY,INT16U size, INT16U fgcolor);
-	void drawString(char *string,INT16U poX, INT16U poY,INT16U size,INT16U fgcolor);
-	void fillRectangle(INT16U poX, INT16U poY, INT16U length, INT16U width, INT16U color);
-
-	void drawLine(INT16U x0,INT16U y0,INT16U x1,INT16U y1,INT16U color);
-	void drawVerticalLine(INT16U poX, INT16U poY,INT16U length,INT16U color);
-	void drawHorizontalLine(INT16U poX, INT16U poY,INT16U length,INT16U color);
-	void drawRectangle(INT16U poX, INT16U poY, INT16U length,INT16U width,INT16U color);
-
-	void drawCircle(int poX, int poY, int r,INT16U color);
-	void fillCircle(int poX, int poY, int r,INT16U color);
-
-	void drawTraingle(int poX1, int poY1, int poX2, int poY2, int poX3, int poY3, INT16U color);
-	INT8U drawNumber(long long_num,INT16U poX, INT16U poY,INT16U size,INT16U fgcolor);
-	INT8U drawFloat(float floatNumber,INT8U decimal,INT16U poX, INT16U poY,INT16U size,INT16U fgcolor);
-	INT8U drawFloat(float floatNumber,INT16U poX, INT16U poY,INT16U size,INT16U fgcolor);
-
-};
-
-extern TFT Tft;
-
-#endif
-
-/*********************************************************************************************************
-  END FILE
-*********************************************************************************************************/
diff --git a/libraries/MultiLCD/MultiLCD.cpp b/libraries/MultiLCD/MultiLCD.cpp
index cb3c3a5..33c753b 100644
--- a/libraries/MultiLCD/MultiLCD.cpp
+++ b/libraries/MultiLCD/MultiLCD.cpp
@@ -70,9 +70,11 @@ void LCD_PCD8544::writeDigit(byte n)
 }
 
 
-void LCD_PCD8544::draw(const unsigned char *data, unsigned char x, unsigned char y, unsigned char width, unsigned char height)
+void LCD_PCD8544::draw(const unsigned char *data, unsigned char width, unsigned char height)
 {
     height >>= 3;
+    unsigned char x = column;
+    unsigned char y = line << 3;
     for (unsigned char y = 0; y < height; y++) {
         setCursor(x, y);
         for (unsigned char x = 0; x < width; x++) {
@@ -92,6 +94,8 @@ void LCD_SSD1306::setCursor(byte column, byte line)
 
 size_t LCD_SSD1306::write(uint8_t c)
 {
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
     if (c == '\n') {
         setCursor(0, m_row + ((m_font == FONT_SIZE_SMALL) ? 1 : 2));
         return 1;
@@ -183,13 +187,14 @@ size_t LCD_SSD1306::write(uint8_t c)
         }
     }
 #endif
+    TWBR = twbrbackup;
     return 1;
 }
 
 void LCD_SSD1306::writeDigit(byte n)
 {
-    //uint8_t twbrbackup = TWBR;
-    //TWBR = 18; // upgrade to 400KHz!
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
     if (m_font == FONT_SIZE_SMALL) {
         Wire.beginTransmission(_i2caddr);
         Wire.write(0x40);
@@ -337,28 +342,27 @@ void LCD_SSD1306::writeDigit(byte n)
         }
         m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 30 : 16;
     }
-    //TWBR = twbrbackup;
+    TWBR = twbrbackup;
 }
 
-void LCD_SSD1306::draw(const PROGMEM byte* buffer, byte x, byte y, byte width, byte height)
+void LCD_SSD1306::draw(const PROGMEM byte* buffer, byte width, byte height)
 {
     ssd1306_command(SSD1306_SETLOWCOLUMN | 0x0);  // low col = 0
     ssd1306_command(SSD1306_SETHIGHCOLUMN | 0x0);  // hi col = 0
     ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // line #0
 
     // save I2C bitrate
-    //uint8_t twbrbackup = TWBR;
-    //TWBR = 18; // upgrade to 400KHz!
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
 
     const PROGMEM byte *p = buffer;
     height >>= 3;
     width >>= 3;
-    y >>= 3;
     for (byte i = 0; i < height; i++) {
       // send a bunch of data in one xmission
-        ssd1306_command(0xB0 + i + y);//set page address
-        ssd1306_command(x & 0xf);//set lower column address
-        ssd1306_command(0x10 | (x >> 4));//set higher column address
+        ssd1306_command(0xB0 + i + m_row);//set page address
+        ssd1306_command(m_col & 0xf);//set lower column address
+        ssd1306_command(0x10 | (m_col >> 4));//set higher column address
 
         for(byte j = 0; j < 8; j++){
             Wire.beginTransmission(_i2caddr);
@@ -369,7 +373,8 @@ void LCD_SSD1306::draw(const PROGMEM byte* buffer, byte x, byte y, byte width, b
             Wire.endTransmission();
         }
     }
-    //TWBR = twbrbackup;
+    TWBR = twbrbackup;
+    m_col += width;
 }
 
 void LCD_SSD1306::clearLine(byte line)
@@ -379,8 +384,8 @@ void LCD_SSD1306::clearLine(byte line)
     ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // line #0
 
     // save I2C bitrate
-    //uint8_t twbrbackup = TWBR;
-    //TWBR = 18; // upgrade to 400KHz!
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
 
     // send a bunch of data in one xmission
     ssd1306_command(0xB0 + line);//set page address
@@ -396,7 +401,7 @@ void LCD_SSD1306::clearLine(byte line)
         Wire.endTransmission();
     }
 
-    //TWBR = twbrbackup;
+    TWBR = twbrbackup;
 }
 
 void LCD_SSD1306::clear(byte x, byte y, byte width, byte height)
@@ -406,8 +411,8 @@ void LCD_SSD1306::clear(byte x, byte y, byte width, byte height)
     ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // line #0
 
     // save I2C bitrate
-    //uint8_t twbrbackup = TWBR;
-    //TWBR = 18; // upgrade to 400KHz!
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
 
     height >>= 3;
     width >>= 3;
@@ -429,5 +434,5 @@ void LCD_SSD1306::clear(byte x, byte y, byte width, byte height)
     }
 
     setCursor(0, 0);
-    //TWBR = twbrbackup;
+    TWBR = twbrbackup;
 }
diff --git a/libraries/MultiLCD/MultiLCD.h b/libraries/MultiLCD/MultiLCD.h
index a5c4085..e51bbf6 100644
--- a/libraries/MultiLCD/MultiLCD.h
+++ b/libraries/MultiLCD/MultiLCD.h
@@ -22,18 +22,23 @@ typedef enum {
 #define FLAG_PIXEL_DOUBLE (FLAG_PIXEL_DOUBLE_H | FLAG_PIXEL_DOUBLE_V)
 
 #define RGB16(r,g,b) (((uint16_t)(r >> 3) << 11) | ((uint16_t)(g >> 2) << 5) | (b >> 2))
+
 #define RGB16_RED 0xF800
 #define RGB16_GREEN 0x7E0
 #define RGB16_BLUE 0x1F
 #define RGB16_YELLOW 0xFFE0
+#define RGB16_CYAN 0x7FF
+#define RGB16_PINK 0xF81F
 #define RGB16_WHITE 0xFFFF
 
+
 extern const PROGMEM unsigned char font5x8[][5];
 extern const PROGMEM unsigned char digits8x8[][8] ;
 extern const PROGMEM unsigned char digits16x16[][32];
 extern const PROGMEM unsigned char digits16x24[][48];
 extern const PROGMEM unsigned char font8x16_doslike[][16];
 extern const PROGMEM unsigned char font8x16_terminal[][16];
+
 #include "PCD8544.h"
 
 class LCD_Common
@@ -43,7 +48,7 @@ public:
     void setFont(FONT_SIZE size) { m_font = size; }
     void setFlags(byte flags) { m_flags = flags; }
     virtual void backlight(bool on) {}
-    virtual void draw(const PROGMEM byte* buffer, byte x, byte y, byte width, byte height) {}
+    virtual void draw(const PROGMEM byte* buffer, byte width, byte height) {}
     void printInt(uint16_t value, int8_t padding = -1);
     void printLong(uint32_t value, int8_t padding = -1);
 protected:
@@ -79,7 +84,7 @@ public:
         setCursor(0, line);
         for (byte i = 14; i > 0; i--) write(' ');
     }
-    void draw(const PROGMEM byte* buffer, byte x, byte y, byte width, byte height);
+    void draw(const PROGMEM byte* buffer, byte width, byte height);
 private:
     void writeDigit(byte n);
 };
@@ -90,7 +95,7 @@ class LCD_SSD1306 : public LCD_Common, public SSD1306, public Print
 {
 public:
     void setCursor(byte column, byte line);
-    void draw(const PROGMEM byte* buffer, byte x, byte y, byte width, byte height);
+    void draw(const PROGMEM byte* buffer, byte width, byte height);
     size_t write(uint8_t c);
     void clear(byte x = 0, byte y = 0, byte width = 128, byte height = 64);
     void clearLine(byte line);
@@ -108,10 +113,10 @@ class LCD_ILI9325D : public LCD_Common, public Print
 {
 public:
     LCD_ILI9325D() { m_font = FONT_SIZE_MEDIUM; }
-    void setCursor(uint16_t column, uint16_t line)
+    void setCursor(uint16_t column, uint8_t line)
     {
         m_y = column;
-        m_x = line * TFT_LINE_HEIGHT;
+        m_x = (uint16_t)line * TFT_LINE_HEIGHT;
     }
     void setTextColor(uint16_t color)
     {
@@ -131,8 +136,8 @@ public:
     }
     void begin();
     void clear(uint16_t x = 0, uint16_t y = 0, uint16_t width = 320, uint16_t height = 240);
-    void draw(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint16_t width, uint16_t height);
-    void draw2x(const PROGMEM byte* buffer, uint16_t x, uint16_t y, byte width, byte height);
+    void draw(const PROGMEM byte* buffer, uint16_t width, uint16_t height);
+    void draw2x(const PROGMEM byte* buffer, byte width, byte height);
     void draw4bpp(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint16_t width, uint16_t height);
     size_t write(uint8_t);
     void clearLine(byte line)
@@ -142,9 +147,9 @@ public:
     byte getLines() { return 53; }
     byte getCols() { return 30; }
 private:
+    void setXY(uint16_t x0,uint16_t x1,uint16_t y0,uint16_t y1);
     void writeDigit(byte n);
     void clearPixels(uint16_t pixels);
-    void setXY(uint16_t x0,uint16_t x1,uint16_t y0,uint16_t y1);
     void WriteData(uint16_t c);
     void WriteData(byte l, byte h);
     void WriteCommandData(uint16_t cmd,uint16_t dat);
@@ -162,10 +167,10 @@ class LCD_ILI9341 : public LCD_Common, public Print
 {
 public:
     LCD_ILI9341() { m_font = FONT_SIZE_MEDIUM; }
-    void setCursor(uint16_t column, uint16_t line)
+    void setCursor(uint16_t column, uint8_t line)
     {
-        m_y = column;
-        m_x = line * TFT_LINE_HEIGHT;
+        m_x = column;
+        m_y = (uint16_t)line * TFT_LINE_HEIGHT;
     }
     void setTextColor(uint16_t color)
     {
@@ -194,15 +199,16 @@ public:
         clear(0, line * TFT_LINE_HEIGHT, 320, 8);
     }
     void begin (void);
-    void setXY(uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
     void setPixel(uint16_t poX, uint16_t poY,uint16_t color);
     void clear(uint16_t XL,uint16_t XR,uint16_t YU,uint16_t YD,uint16_t color = 0);
     void clear(void);
     size_t write(uint8_t);
     void backlight(bool on);
-    void draw(const PROGMEM byte* buffer, uint16_t x, uint16_t y, uint16_t width, uint16_t height);
-    void draw2x(const PROGMEM byte* buffer, uint16_t x, uint16_t y, byte width, byte height);
+    void draw(const PROGMEM byte* buffer, uint16_t width, uint16_t height);
+    void draw2x(const PROGMEM byte* buffer, byte width, byte height);
 private:
+    void setXY(uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
+    void sendPixelData(byte d);
     void writeDigit(byte n);
     void clearPixels(uint16_t pixels);
     void setCol(uint16_t StartCol,uint16_t EndCol);
@@ -217,3 +223,22 @@ private:
     uint16_t m_x;
     uint16_t m_y;
 };
+
+class LCD_SH1106 : public LCD_Common, public Print
+{
+public:
+    void begin();
+    void setCursor(byte column, byte line);
+    void draw(const PROGMEM byte* buffer, byte width, byte height);
+    size_t write(uint8_t c);
+    void clear(byte x = 0, byte y = 0, byte width = 128, byte height = 64);
+    void clearLine(byte line);
+    byte getLines() { return 21; }
+    byte getCols() { return 8; }
+private:
+    void WriteCommand(unsigned char ins);
+    void WriteData(unsigned char dat);
+    void writeDigit(byte n);
+    byte m_col;
+    byte m_row;
+};
diff --git a/libraries/MultiLCD/SH1106.cpp b/libraries/MultiLCD/SH1106.cpp
new file mode 100644
index 0000000..0496342
--- /dev/null
+++ b/libraries/MultiLCD/SH1106.cpp
@@ -0,0 +1,401 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include "MultiLCD.h"
+
+#define I2C_ADDR 0x78 >> 1
+
+void LCD_SH1106::WriteCommand(unsigned char ins)
+{
+  Wire.beginTransmission(I2C_ADDR);//0x78 >> 1
+  Wire.write(0x00);//0x00
+  Wire.write(ins);
+  Wire.endTransmission();
+}
+
+void LCD_SH1106::WriteData(unsigned char dat)
+{
+  Wire.beginTransmission(I2C_ADDR);//0x78 >> 1
+  Wire.write(0x40);//0x40
+  Wire.write(dat);
+  Wire.endTransmission();
+}
+
+void LCD_SH1106::setCursor(unsigned char x, unsigned char y)
+{
+    m_col = x + 2;
+    m_row = y;
+    WriteCommand(0xb0 + m_row);
+    WriteCommand(m_col & 0xf);//set lower column address
+    WriteCommand(0x10 | (m_col >> 4));//set higher column address
+}
+
+void LCD_SH1106::clear(byte x, byte y, byte width, byte height)
+{
+    WriteCommand(SSD1306_SETLOWCOLUMN | 0x0);  // low col = 0
+    WriteCommand(SSD1306_SETHIGHCOLUMN | 0x0);  // hi col = 0
+    WriteCommand(SSD1306_SETSTARTLINE | 0x0); // line #0
+
+    // save I2C bitrate
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
+
+    height >>= 3;
+    width >>= 3;
+    y >>= 3;
+    for (byte i = 0; i < height; i++) {
+      // send a bunch of data in one xmission
+        WriteCommand(0xB0 + i + y);//set page address
+        WriteCommand((x + 2) & 0xf);//set lower column address
+        WriteCommand(0x10 | (x >> 4));//set higher column address
+
+        for(byte j = 0; j < 8; j++){
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte k = 0; k < width; k++) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+        }
+    }
+
+    setCursor(0, 0);
+    TWBR = twbrbackup;
+}
+
+size_t LCD_SH1106::write(uint8_t c)
+{
+    if (c == '\n') {
+        setCursor(0, m_row + ((m_font == FONT_SIZE_SMALL) ? 1 : 2));
+        return 1;
+    } else if (c == '\r') {
+        m_col = 0;
+        return 1;
+    }
+
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
+#ifndef MEMORY_SAVING
+    if (m_font == FONT_SIZE_SMALL) {
+#endif
+        Wire.beginTransmission(I2C_ADDR);
+        Wire.write(0x40);
+        if (c > 0x20 && c < 0x7f) {
+            c -= 0x21;
+            for (byte i = 0; i < 5; i++) {
+                byte d = pgm_read_byte(&font5x8[c][i]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.write(0);
+        } else {
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 11 : 6; i > 0; i--) {
+                Wire.write(0);
+            }
+        }
+        Wire.endTransmission();
+        m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 11 : 6;
+        if (m_col >= 128) {
+            m_col = 0;
+            m_row ++;
+        }
+#ifndef MEMORY_SAVING
+    } else {
+        if (c > 0x20 && c < 0x7f) {
+            c -= 0x21;
+
+            WriteCommand(0xB0 + m_row);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = 0; i <= 14; i += 2) {
+                byte d = pgm_read_byte(&font8x16_terminal[c][i]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 1);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = 1; i <= 15; i += 2) {
+                byte d = pgm_read_byte(&font8x16_terminal[c][i]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+        } else {
+            WriteCommand(0xB0 + m_row);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 16 : 8; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 1);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 16 : 8; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+        }
+        m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 17 : 9;
+        if (m_col >= 128) {
+            m_col = 0;
+            m_row += 2;
+        }
+    }
+#endif
+    TWBR = twbrbackup;
+    return 1;
+}
+
+void LCD_SH1106::writeDigit(byte n)
+{
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
+
+    if (m_font == FONT_SIZE_SMALL) {
+        Wire.beginTransmission(I2C_ADDR);
+        Wire.write(0x40);
+        if (n <= 9) {
+            n += '0' - 0x21;
+            for (byte i = 0; i < 5; i++) {
+                Wire.write(pgm_read_byte(&font5x8[n][i]));
+            }
+            Wire.write(0);
+        } else {
+            for (byte i = 0; i < 6; i++) {
+                Wire.write(0);
+            }
+        }
+        Wire.endTransmission();
+        m_col += 6;
+    } else if (m_font == FONT_SIZE_MEDIUM) {
+        write(n <= 9 ? ('0' + n) : ' ');
+#ifndef MEMORY_SAVING
+    } else if (m_font == FONT_SIZE_LARGE) {
+        if (n <= 9) {
+            byte i;
+            WriteCommand(0xB0 + m_row);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (i = 0; i < 16; i ++) {
+                byte d = pgm_read_byte(&digits16x16[n][i]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 1);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (; i < 32; i ++) {
+                byte d = pgm_read_byte(&digits16x16[n][i]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+        } else {
+            WriteCommand(0xB0 + m_row);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 1);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+        }
+        m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 30 : 16;
+#endif
+    } else {
+        if (n <= 9) {
+            byte i;
+            WriteCommand(0xB0 + m_row);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (i = 0; i < 16; i ++) {
+                byte d = pgm_read_byte(&digits16x24[n][i * 3]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 1);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (i = 0; i < 16; i ++) {
+                byte d = pgm_read_byte(&digits16x24[n][i * 3 + 1]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 2);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (i = 0; i < 16; i ++) {
+                byte d = pgm_read_byte(&digits16x24[n][i * 3 + 2]);
+                Wire.write(d);
+                if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d);
+            }
+            Wire.endTransmission();
+        } else {
+            WriteCommand(0xB0 + m_row);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 1);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+
+            WriteCommand(0xB0 + m_row + 2);//set page address
+            WriteCommand(m_col & 0xf);//set lower column address
+            WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) {
+                Wire.write(0);
+            }
+            Wire.endTransmission();
+        }
+        m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 30 : 16;
+    }
+    TWBR = twbrbackup;
+}
+
+void LCD_SH1106::draw(const PROGMEM byte* buffer, byte width, byte height)
+{
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
+
+    WriteCommand(SSD1306_SETLOWCOLUMN | 0x0);  // low col = 0
+    WriteCommand(SSD1306_SETHIGHCOLUMN | 0x0);  // hi col = 0
+    WriteCommand(SSD1306_SETSTARTLINE | 0x0); // line #0
+
+    const PROGMEM byte *p = buffer;
+    height >>= 3;
+    width >>= 3;
+    for (byte i = 0; i < height; i++) {
+      // send a bunch of data in one xmission
+        WriteCommand(0xB0 + i + m_row);//set page address
+        WriteCommand(m_col & 0xf);//set lower column address
+        WriteCommand(0x10 | (m_col >> 4));//set higher column address
+
+        for(byte j = 0; j < 8; j++){
+            Wire.beginTransmission(I2C_ADDR);
+            Wire.write(0x40);
+            for (byte k = 0; k < width; k++, p++) {
+                Wire.write(pgm_read_byte(p));
+            }
+            Wire.endTransmission();
+        }
+    }
+    TWBR = twbrbackup;
+    m_col += width;
+}
+
+void LCD_SH1106::begin()
+{
+  Wire.begin();
+
+  WriteCommand(0xAE);    /*display off*/
+
+  WriteCommand(0x02);    /*set lower column address*/
+  WriteCommand(0x10);    /*set higher column address*/
+
+  WriteCommand(0x40);    /*set display start line*/
+
+  WriteCommand(0xB0);    /*set page address*/
+
+  WriteCommand(0x81);    /*contract control*/
+  WriteCommand(0x80);    /*128*/
+
+  WriteCommand(0xA1);    /*set segment remap*/
+
+  WriteCommand(0xA6);    /*normal / reverse*/
+
+  WriteCommand(0xA8);    /*multiplex ratio*/
+  WriteCommand(0x3F);    /*duty = 1/32*/
+
+  WriteCommand(0xad);    /*set charge pump enable*/
+  WriteCommand(0x8b);     /*external VCC   */
+
+  WriteCommand(0x30);    /*0X30---0X33  set VPP   9V liangdu!!!!*/
+
+  WriteCommand(0xC8);    /*Com scan direction*/
+
+  WriteCommand(0xD3);    /*set display offset*/
+  WriteCommand(0x00);   /*   0x20  */
+
+  WriteCommand(0xD5);    /*set osc division*/
+  WriteCommand(0x80);
+
+  WriteCommand(0xD9);    /*set pre-charge period*/
+  WriteCommand(0x1f);    /*0x22*/
+
+  WriteCommand(0xDA);    /*set COM pins*/
+  WriteCommand(0x12);
+
+  WriteCommand(0xdb);    /*set vcomh*/
+  WriteCommand(0x40);
+
+  WriteCommand(0xAF);    /*display ON*/
+
+  clear();
+}
diff --git a/libraries/MultiLCD/SSD1306.cpp b/libraries/MultiLCD/SSD1306.cpp
index 5fc1ba0..7b1c4f6 100644
--- a/libraries/MultiLCD/SSD1306.cpp
+++ b/libraries/MultiLCD/SSD1306.cpp
@@ -238,8 +238,8 @@ void SSD1306::fill(unsigned char dat)
     ssd1306_command(0x10);//set higher column address
     ssd1306_command(0xB0);//set page address
 
-    //uint8_t twbrbackup = TWBR;
-    //TWBR = 18; // upgrade to 400KHz!
+    uint8_t twbrbackup = TWBR;
+    TWBR = 18; // upgrade to 400KHz!
     for (byte i=0; i<(SSD1306_LCDHEIGHT/8); i++)
     {
         // send a bunch of data in one xmission
@@ -256,7 +256,7 @@ void SSD1306::fill(unsigned char dat)
             Wire.endTransmission();
         }
     }
-    //TWBR = twbrbackup;
+    TWBR = twbrbackup;
 }
 
 void SSD1306::draw8x8(byte* buffer, uint8_t x, uint8_t y)