C.Dupaty 03-2018 test on NUCLEO STM32-F411RE LCD 2x16, I2C interface Chinese model, adress I2C=0x4E (search on AliExpress for IIC/I2C 1602 Module) Important ! configure in TextLCD_Config.h : Valid only one of the lines : #define DFROBOT 0 // chinese OK #define YWROBOT 0 // chinese 0K #define SYDZ 1 // chinese OK Font is the same as http://www.farnell.com/datasheets/2362518.pdf

Fork of TextLCD by Wim Huiskamp

/* Hello World! for the TextLCD Enhanced Library C.Dupaty 03-2018 test on NUCLEO STM32-F411RE LCD 2x16, I2C interface Chinese model, adress I2C=0x4E (search on AliExpress for IIC/I2C 1602 Module) Important ! configure in TextLCD_Config.h : Valid only one of the lines :

  1. define DFROBOT 0 chinese OK
  2. define YWROBOT 0 chinese 0K
  3. define SYDZ 1 chinese OK Font is the same as http://www.farnell.com/datasheets/2362518.pdf
  • /
Revision:
15:b70ebfffb258
Parent:
14:0c32b66b14b8
Child:
16:c276b75e6585
--- a/TextLCD.cpp	Sun Feb 10 18:43:51 2013 +0000
+++ b/TextLCD.cpp	Tue Feb 19 22:09:09 2013 +0000
@@ -2,6 +2,7 @@
  * Copyright (c) 2007-2010, sford, http://mbed.org
  *               2013, v01: WH, Added LCD types, fixed LCD address issues, added Cursor and UDCs 
  *               2013, v02: WH, Added I2C and SPI bus interfaces  
+ *               2013, v03: WH, Added support for LCD40x4 which uses 2 controllers 
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -25,13 +26,21 @@
 #include "TextLCD.h"
 #include "mbed.h"
 
+
+/* Create a TextLCD interface for using regular mbed pins
+ *
+ * @param rs    Instruction/data control line
+ * @param e     Enable line (clock)
+ * @param d4-d7 Data lines for using as a 4-bit interface
+ * @param type  Sets the panel size/addressing mode (default = LCD16x2)
+ * @param e2    Enable2 line (clock for second controller, LCD40x4 only) 
+ */ 
 TextLCD::TextLCD(PinName rs, PinName e,
                  PinName d4, PinName d5, PinName d6, PinName d7,
-                 LCDType type): _rs(rs), _e(e),
-                                _d(d4, d5, d6, d7),
-                                _cs(NC),                                                
-                                _type(type) {
-
+                 LCDType type, PinName e2) : _rs(rs), _e(e), _e2(e2),
+                                             _d(d4, d5, d6, d7),
+                                             _cs(NC), 
+                                             _type(type) {
 
   _busType = _PinBus;
 
@@ -39,19 +48,25 @@
 
 }
 
-
+/* Create a TextLCD interface using an I2C PC8574 portexpander
+ *
+ * @param i2c             I2C Bus
+ * @param deviceAddress   I2C slave address (PCF8574)
+ * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+ */
 TextLCD::TextLCD(I2C *i2c, char deviceAddress, LCDType type) :
-        _rs(NC), _e(NC), _d(NC),
-        _cs(NC),                
-        _i2c(i2c),
+        _rs(NC), _e(NC), _e2(NC),
+        _d(NC),
+        _i2c(i2c),        
+        _cs(NC),
         _type(type) {
-        
+         
   _slaveAddress = deviceAddress;
   _busType = _I2CBus;
 
   
   // Init the portexpander bus
-  _lcd_bus = 0x80;
+  _lcd_bus = D_LCD_BUS_DEF;
   
   // write the new data to the portexpander
   _i2c->write(_slaveAddress, &_lcd_bus, 1);    
@@ -60,11 +75,17 @@
     
 }
 
-
+ /* Create a TextLCD interface using an SPI 74595 portexpander
+  *
+  * @param spi             SPI Bus
+  * @param cs              chip select pin (active low)
+  * @param type            Sets the panel size/addressing mode (default = LCD16x2)
+  */
 TextLCD::TextLCD(SPI *spi, PinName cs, LCDType type) :
-        _rs(NC), _e(NC), _d(NC),
-        _spi(spi),
-        _cs(cs),        
+        _rs(NC), _e(NC), _e2(NC),
+        _d(NC),
+        _spi(spi),        
+        _cs(cs),
         _type(type) {
         
   _busType = _SPIBus;
@@ -77,7 +98,7 @@
 
 
   // Init the portexpander bus
-  _lcd_bus = 0x80;
+  _lcd_bus = D_LCD_BUS_DEF;
   
   // write the new data to the portexpander
   _setCS(false);  
@@ -89,47 +110,75 @@
 }
 
 
-/*  Init the LCD controller
- *  4-bit mode, number of lines, no cursor etc
+/*  Init the LCD Controller(s)
  *  Clear display 
  */
 void TextLCD::_init() {
-//    _e  = 1;  
-//    _rs = 0;            // command mode
+  
+  // Select and configure second LCD controller when needed
+  if(_type==LCD40x4) {
+    _ctrl=TextLCD::_LCDCtrl_1; // Select 2nd controller
+    
+    _initCtrl();               // Init 2nd controller
+    
+    // Secondary LCD controller Clearscreen
+    _writeCommand(0x01);       // cls, and set cursor to 0    
+    wait_ms(10);     // The CLS command takes 1.64 ms.
+                     // Since we are not using the Busy flag, Lets be safe and take 10 ms
+    
+  }
+    
+  // Select and configure primary LCD controller
+  _ctrl=TextLCD::_LCDCtrl_0; // Select primary controller  
 
-    _setEnable(true);    
+  _initCtrl();               // Init primary controller
+  
+  // Primary LCD controller Clearscreen
+  _writeCommand(0x01);       // cls, and set cursor to 0
+
+  wait_ms(10);     // The CLS command takes 1.64 ms.
+                   // Since we are not using the Busy flag, Lets be safe and take 10 ms
+    
+} 
+
+/*  Init the LCD controller
+ *  4-bit mode, number of lines, fonttype, no cursor etc
+ *  
+ */
+void TextLCD::_initCtrl() {
+
     _setRS(false);      // command mode
     
-//    wait(0.015);        // Wait 15ms to ensure powered up
-    wait_ms(15);        // Wait 15ms to ensure powered up
+    wait_ms(20);        // Wait 20ms to ensure powered up
 
     // send "Display Settings" 3 times (Only top nibble of 0x30 as we've got 4-bit bus)
     for (int i=0; i<3; i++) {
         _writeByte(0x3);
-//        wait(0.00164);  // this command takes 1.64ms, so wait for it
-        wait_ms(10);    // this command takes 1.64ms, so wait for it 
+        wait_ms(15);    // this command takes 1.64ms, so wait for it 
     }
     _writeByte(0x2);     // 4-bit mode
-//    wait(0.000040f);     // most instructions take 40us
     wait_us(40);         // most instructions take 40us
     
     // Display is now in 4-bit mode
     switch (_type) {
         case LCD8x1:
-            _writeCommand(0x20); // Function set 001 BW N F - -
+            _writeCommand(0x20); // Function set 001 DL N F - -
+                                 //  DL=0 (4 bits bus)             
                                  //   N=0 (1 line)
                                  //   F=0 (5x7 dots font)
             break;                                
             
         case LCD24x4:
             // Special mode for KS0078
-            _writeCommand(0x2A); // Function set 001 BW N RE DH REV
+            _writeCommand(0x2A); // Function set 001 DL N RE DH REV
+                                 //   DL=0  (4 bits bus)             
                                  //    N=1  (Dont care for KS0078)
                                  //   RE=0  (Extended Regs, special mode for KS0078)
                                  //   DH=1  (Disp shift, special mode for KS0078)                                
                                  //   REV=0 (Reverse, special mode for KS0078)
 
-            _writeCommand(0x2E); // Function set 001 BW N RE DH REV
+            _writeCommand(0x2E); // Function set 001 DL N RE DH REV
+                                 //   DL=0  (4 bits bus)             
                                  //    N=1  (Dont care for KS0078)
                                  //   RE=1  (Ena Extended Regs, special mode for KS0078)
                                  //   DH=1  (Disp shift, special mode for KS0078)                                
@@ -140,17 +189,20 @@
                                  //   BW=0  (Cur BW invert disable, special mode for KS0078)
                                  //   NW=1  (4 Line, special mode for KS0078)                                
 
-            _writeCommand(0x2A); // Function set 001 BW N RE DH REV
+            _writeCommand(0x2A); // Function set 001 DL N RE DH REV
+                                 //   DL=0  (4 bits bus)             
                                  //    N=1  (Dont care for KS0078)
                                  //   RE=0  (Dis. Extended Regs, special mode for KS0078)
                                  //   DH=1  (Disp shift, special mode for KS0078)                                
                                  //   REV=0 (Reverse, special mode for KS0078)
             break;
                                             
+// All other LCD types are initialised as 2 Line displays (including LCD40x4)
         default:
-            _writeCommand(0x28); // Function set 001 BW N F - -
+            _writeCommand(0x28); // Function set 001 DL N F - -
+                                 //  DL=0 (4 bits bus) 
                                  //   N=1 (2 lines)
-                                 //   F=0 (5x7 dots font)
+                                 //   F=0 (5x7 dots font, only option for 2 line display)
                                  //    -  (Don't care)                                
             
             break;
@@ -163,33 +215,117 @@
 
 //    _writeCommand(0x0C); // Display Ctrl 0000 1 D C B
 //                         //   Display On, Cursor Off, Blink Off
-    setCursor(TextLCD::CurOff_BlkOff);  
-    
-    cls();    
+    setCursor(TextLCD::CurOff_BlkOff);     
+
 }
 
 
-void TextLCD::_character(int column, int row, int c) {
-    int addr = getAddress(column, row);
-    
-    _writeCommand(0x80 | addr);
-    _writeData(c);
+ 
+#if(LCD40x4Test)
+void TextLCD::cls() {
+
+  // Select and configure second LCD controller when needed
+  if(_type==LCD40x4) {
+    _ctrl=TextLCD::_LCDCtrl_1; // Select 2nd controller
+
+    // Second LCD controller Cursor always Off
+    _setCursor(TextLCD::CurOff_BlkOff);
+
+    // Second LCD controller Clearscreen
+    _writeCommand(0x01); // cls, and set cursor to 0    
+
+    wait_ms(10);     // The CLS command takes 1.64 ms.
+                     // Since we are not using the Busy flag, Lets be safe and take 10 ms
+
+  
+    _ctrl=TextLCD::_LCDCtrl_0; // Select primary controller
+  }
+  
+  // Primary LCD controller Clearscreen
+  _writeCommand(0x01); // cls, and set cursor to 0
+
+  wait_ms(10);     // The CLS command takes 1.64 ms.
+                   // Since we are not using the Busy flag, Lets be safe and take 10 ms
+
+  // Restore cursormode on primary LCD controller when needed
+  if(_type==LCD40x4) {
+    _setCursor(_currentCursor);
+  }
+                   
+  _row=0;          // Reset Cursor location
+  _column=0;
 }
 
-
+#else
+//standard
 void TextLCD::cls() {
     _writeCommand(0x01); // cls, and set cursor to 0
-//    wait(0.00164f);      // This command takes 1.64 ms
+
     wait_ms(10);     // The CLS command takes 1.64 ms.
                      // Since we are not using the Busy flag, Lets be safe and take 10 ms
     locate(0, 0);
 }
+#endif
 
 void TextLCD::locate(int column, int row) {
-    _column = column;
-    _row = row;
+    
+   // setAddress() does all the heavy lifting:
+   //   check column and row sanity, 
+   //   switch controllers for LCD40x4 if needed
+   //   switch cursor for LCD40x4 if needed
+   //   set the new memory address to show cursor at correct location
+   setAddress(column, row);
+       
+}
+    
+
+//Not needed in new version, is now part of _putc()
+void TextLCD::_character(int column, int row, int c) {
+  int addr = getAddress(column, row);
+    
+  _writeCommand(0x80 | addr);
+  _writeData(c);
 }
 
+
+#if(LCD40x4Test)
+
+int TextLCD::_putc(int value) {
+  int addr;
+    
+    if (value == '\n') {
+      //No character to write
+      
+      //Update Cursor      
+      _column = 0;
+      _row++;
+      if (_row >= rows()) {
+        _row = 0;
+      }      
+    }
+    else {
+      //Character to write      
+      _writeData(value); 
+              
+      //Update Cursor
+      _column++;
+      if (_column >= columns()) {
+        _column = 0;
+        _row++;
+        if (_row >= rows()) {
+          _row = 0;
+        }
+      }          
+    } //else
+
+    //Set next memoryaddress, make sure cursor blinks at next location
+    addr = getAddress(_column, _row);
+    _writeCommand(0x80 | addr);
+            
+    return value;
+}
+#else
+//Standard
 int TextLCD::_putc(int value) {
     if (value == '\n') {
         _column = 0;
@@ -208,9 +344,16 @@
             }
         }
     }
+       
     return value;
 }
 
+#endif
+
+
+
+
+
 int TextLCD::_getc() {
     return -1;
 }
@@ -220,31 +363,79 @@
 
   switch(_busType) {
     case _PinBus : 
+#if(LCD40x4Test)
+                    if(_ctrl==TextLCD::_LCDCtrl_0) {
+                      if (value)
+                        _e  = 1;    // Set E bit 
+                      else  
+                        _e  = 0;    // Reset E bit  
+                    }    
+                    else {   
+                      if (value)
+                        _e2 = 1;    // Set E2 bit 
+                      else  
+                        _e2 = 0;    // Reset E2 bit  
+                    }    
+
+#else
                     if (value)
                       _e  = 1;    // Set E bit 
                     else  
                       _e  = 0;    // Reset E bit  
-
+#endif
                     break;  
     
     case _I2CBus : 
+    
+#if(LCD40x4Test)
+                   if(_ctrl==TextLCD::_LCDCtrl_0) {
+                     if (value)
+                       _lcd_bus |= D_LCD_E;     // Set E bit 
+                     else                     
+                       _lcd_bus &= ~D_LCD_E;    // Reset E bit                     
+                   }
+                   else {
+                     if (value)
+                       _lcd_bus |= D_LCD_E2;    // Set E2 bit 
+                     else                     
+                       _lcd_bus &= ~D_LCD_E2;   // Reset E2bit                     
+                   }    
+
+#else
                    if (value)
                      _lcd_bus |= D_LCD_E;     // Set E bit 
                    else                     
                      _lcd_bus &= ~D_LCD_E;    // Reset E bit                     
 
-                   // write the new data to the portexpander
+#endif                   
+                   // write the new data to the I2C portexpander
                    _i2c->write(_slaveAddress, &_lcd_bus, 1);    
-                   
+
                    break;  
     
     case _SPIBus :
+#if(LCD40x4Test)
+                   if(_ctrl==TextLCD::_LCDCtrl_0) {
+                     if (value)
+                       _lcd_bus |= D_LCD_E;     // Set E bit 
+                     else                     
+                       _lcd_bus &= ~D_LCD_E;    // Reset E bit                     
+                   }
+                   else {
+                     if (value)
+                       _lcd_bus |= D_LCD_E2;    // Set E2 bit 
+                     else                     
+                       _lcd_bus &= ~D_LCD_E2;   // Reset E2 bit                     
+                   }
+                  
+#else    
                    if (value)
                      _lcd_bus |= D_LCD_E;     // Set E bit 
                    else                     
                      _lcd_bus &= ~D_LCD_E;    // Reset E bit                     
-         
-                   // write the new data to the portexpander
+#endif         
+
+                   // write the new data to the SPI portexpander
                    _setCS(false);  
                    _spi->write(_lcd_bus);   
                    _setCS(true);  
@@ -270,7 +461,7 @@
                    else                     
                      _lcd_bus &= ~D_LCD_RS;   // Reset RS bit                     
 
-                   // write the new data to the portexpander
+                   // write the new data to the I2C portexpander
                    _i2c->write(_slaveAddress, &_lcd_bus, 1);    
                    
                    break;
@@ -281,7 +472,7 @@
                    else                     
                      _lcd_bus &= ~D_LCD_RS;   // Reset RS bit                     
       
-                   // write the new data to the portexpander
+                   // write the new data to the SPI portexpander
                    _setCS(false);  
                    _spi->write(_lcd_bus);   
                    _setCS(true);  
@@ -322,7 +513,7 @@
                     else                     
                       _lcd_bus &= ~D_LCD_D7;  // Reset Databit                     
                     
-                    // write the new data to the portexpander
+                    // write the new data to the I2C portexpander
                     _i2c->write(_slaveAddress, &_lcd_bus, 1);  
                    
                     break;                    
@@ -350,7 +541,7 @@
                     else                     
                       _lcd_bus &= ~D_LCD_D7;  // Reset Databit                     
                     
-                   // write the new data to the portexpander
+                   // write the new data to the SPI portexpander
                    _setCS(false);  
                    _spi->write(_lcd_bus);   
                    _setCS(true);  
@@ -364,48 +555,51 @@
 // Set CS line. Only used for SPI bus
 void TextLCD::_setCS(bool value) {
 
-  if (value)
+  if (value) {   
     _cs  = 1;    // Set CS pin 
+  }  
   else  
     _cs  = 0;    // Reset CS pin 
-
+  
 }
 
 
 
 void TextLCD::_writeByte(int value) {
-//    _d = value >> 4;
-    _setData(value >> 4);
-//    wait(0.000040f); // most instructions take 40us
-    wait_us(40); // most instructions take 40us    
-//    _e = 0;
-    _setEnable(false);
-//    wait(0.000040f);
-    wait_us(40); // most instructions take 40us        
-//    _e = 1;
+
+// Enable is Low
+    _setEnable(true);          
+    _setData(value >> 4);   
+    wait_us(1); // Data setup time    
+    _setEnable(false);   
+    wait_us(1); // Data hold time
+    
     _setEnable(true);        
-//    _d = value >> 0;
     _setData(value >> 0);    
-//    wait(0.000040f);
-    wait_us(40); // most instructions take 40us        
-//    _e = 0;
+    wait_us(1); // Data setup time        
     _setEnable(false);    
-//    wait(0.000040f);  // most instructions take 40us
-    wait_us(40); // most instructions take 40us        
-//    _e = 1;
-    _setEnable(true);    
+    wait_us(1); // Datahold time
+
+// Enable is Low
+
 }
 
 void TextLCD::_writeCommand(int command) {
-//    _rs = 0;
+
     _setRS(false);        
-    _writeByte(command);
+    wait_us(1);  // Data setup time            
+    
+    _writeByte(command);   
+    wait_us(40); // most instructions take 40us            
 }
 
 void TextLCD::_writeData(int data) {
-//    _rs = 1;
+
     _setRS(true);            
+    wait_us(1);  // Data setup time        
+        
     _writeByte(data);
+    wait_us(40); // data writes take 40us                
 }
 
 
@@ -459,6 +653,18 @@
             else   
               return 0x40 + (column - 8);                        
 
+        case LCD12x4:
+            switch (row) {
+                case 0:
+                    return 0x00 + column;
+                case 1:
+                    return 0x40 + column;
+                case 2:
+                    return 0x0C + column;
+                case 3:
+                    return 0x4C + column;
+            }
+
         case LCD16x4:
             switch (row) {
                 case 0:
@@ -501,11 +707,50 @@
             return 0x00 + (row * 40) + column;
       
         case LCD8x2:        
+        case LCD12x2:                
         case LCD16x2:
         case LCD20x2:
         case LCD24x2:        
         case LCD40x2:                
             return 0x00 + (row * 0x40) + column;
+
+#if(LCD40x4Test)
+        case LCD40x4:                
+          // LCD40x4 is a special case since it has 2 controllers
+          // Each controller is configured as 40x2
+          if (row<2) { 
+            // Test to see if we need to switch between controllers  
+            if (_ctrl != _LCDCtrl_0) {
+              // Second LCD controller Cursor Off
+              _setCursor(TextLCD::CurOff_BlkOff);
+
+              // Select primary controller
+              _ctrl = _LCDCtrl_0;
+
+              // Restore cursormode on primary LCD controller
+              _setCursor(_currentCursor);
+            }           
+            
+            return 0x00 + (row * 0x40) + column;          
+          }
+          else {
+
+            // Test to see if we need to switch between controllers  
+            if (_ctrl != _LCDCtrl_1) {
+              // Primary LCD controller Cursor Off
+              _setCursor(TextLCD::CurOff_BlkOff);
+
+              // Select secondary controller
+              _ctrl = _LCDCtrl_1;
+
+              // Restore cursormode on secondary LCD controller
+              _setCursor(_currentCursor);
+            }           
+                                   
+            return 0x00 + ((row-2) * 0x40) + column;          
+          } 
+          
+#endif
             
 // Should never get here.
         default:            
@@ -514,13 +759,31 @@
 }
 
 
-// Added for consistency. Set row, column and update memoryaddress.
+// Set row, column and update memoryaddress.
 //
 void TextLCD::setAddress(int column, int row) {
-
-    locate(column, row);
+   
+// Sanity Check column
+    if (column < 0) {
+      _column = 0;
+    }
+    else if (column >= columns()) {
+      _column = columns() - 1;
+    } else _column = column;
     
-    int addr = getAddress(column, row);
+// Sanity Check row
+    if (row < 0) {
+      _row = 0;
+    }
+    else if (row >= rows()) {
+      _row = rows() - 1;
+    } else _row = row;
+    
+    
+// Compute the memory address
+// For LCD40x4:  switch controllers if needed
+//               switch cursor if needed
+    int addr = getAddress(_column, _row);
     
     _writeCommand(0x80 | addr);
 }
@@ -530,6 +793,10 @@
         case LCD8x1:
         case LCD8x2:
             return 8;
+        
+        case LCD12x2:        
+        case LCD12x4:        
+            return 12;        
 
         case LCD16x1:        
         case LCD16x2:
@@ -546,6 +813,10 @@
             return 24;        
 
         case LCD40x2:
+
+#if(LCD40x4Test)
+        case LCD40x4:
+#endif        
             return 40;        
         
 // Should never get here.
@@ -560,7 +831,8 @@
         case LCD16x1:         
             return 1;           
 
-        case LCD8x2:        
+        case LCD8x2:  
+        case LCD12x2:                      
         case LCD16x2:
         case LCD16x2B:
         case LCD20x2:
@@ -568,9 +840,13 @@
         case LCD40x2:                
             return 2;
                     
+        case LCD12x4:        
         case LCD16x4:
         case LCD20x4:
         case LCD24x4:        
+#if(LCD40x4Test)
+        case LCD40x4:
+#endif        
             return 4;
 
 // Should never get here.      
@@ -580,27 +856,33 @@
 }
 
 
+
+#if(LCD40x4Test)
+
 void TextLCD::setCursor(TextLCD::LCDCursor show) { 
+
+  // Save new cursor mode, needed when 2 controllers are in use
+  _currentCursor = show;
     
+  // Configure current LCD controller
+  _setCursor(_currentCursor);
+    
+}
+
+void TextLCD::_setCursor(TextLCD::LCDCursor show) { 
+    
+    // Configure current LCD controller    
     switch (show) {
       case CurOff_BlkOff : _writeCommand(0x0C); // Cursor off and Blink Off
-                           wait_us(40);
-                           _cursor = show;
                            break;
 
-      case CurOn_BlkOff   : _writeCommand(0x0E); // Cursor on and Blink Off
-                           wait_us(40);  
-                           _cursor = show;
+      case CurOn_BlkOff :  _writeCommand(0x0E); // Cursor on and Blink Off
                            break;
 
       case CurOff_BlkOn :  _writeCommand(0x0D); // Cursor off and Blink On
-                           wait_us(40);
-                           _cursor = show;
                            break;
 
-      case CurOn_BlkOn   : _writeCommand(0x0F); // Cursor on and Blink char
-                           wait_us(40);  
-                           _cursor = show;
+      case CurOn_BlkOn  :  _writeCommand(0x0F); // Cursor on and Blink char
                            break;
 
 // Should never get here.
@@ -611,13 +893,96 @@
 
 }
 
+#else
+//standard
+void TextLCD::setCursor(TextLCD::LCDCursor show) { 
+    
+    switch (show) {
+      case CurOff_BlkOff : _writeCommand(0x0C); // Cursor off and Blink Off
+                           break;
 
+      case CurOn_BlkOff  : _writeCommand(0x0E); // Cursor on and Blink Off
+                           break;
+
+      case CurOff_BlkOn :  _writeCommand(0x0D); // Cursor off and Blink On
+                           break;
+
+      case CurOn_BlkOn   : _writeCommand(0x0F); // Cursor on and Blink char
+                           break;
+
+// Should never get here.
+      default : 
+                           break;
+                      
+    }
+
+}
+
+#endif
+
+
+
+
+#if(LCD40x4Test)
 void TextLCD::setUDC(unsigned char c, char *udc_data) {
-  _writeCommand(0x40 + ((c & 0x07) << 3)); //Set CG-RAM address
+  
+  // Select and configure second LCD controller when needed
+  if(_type==LCD40x4) {
+    _LCDCtrl current_ctrl = _ctrl; // Temp save current controller
+   
+    // Select primary controller     
+    _ctrl=TextLCD::_LCDCtrl_0;
+    
+    // Configure primary LCD controller
+    _setUDC(c, udc_data);
+
+    // Select 2nd controller
+    _ctrl=TextLCD::_LCDCtrl_1;
+  
+    // Configure secondary LCD controller    
+    _setUDC(c, udc_data);
 
+    // Restore current controller
+    _ctrl=current_ctrl;       
+  }
+  else {
+    // Configure primary LCD controller
+    _setUDC(c, udc_data); 
+  }
+    
+}
+
+void TextLCD::_setUDC(unsigned char c, char *udc_data) {
+  
+  // Select CG RAM for current LCD controller
+  _writeCommand(0x40 + ((c & 0x07) << 3)); //Set CG-RAM address,
+                                           //8 sequential locations needed per UDC
+  // Store UDC pattern 
   for (int i=0; i<8; i++) {
     _writeData(*udc_data++);
   }
+   
+  //Select DD RAM again for current LCD controller
+  int addr = getAddress(_column, _row);
+  _writeCommand(0x80 | addr);
+  
 }
 
+#else
+//standard
+void TextLCD::setUDC(unsigned char c, char *udc_data) {
+  // Select CG RAM for current LCD controller
+  _writeCommand(0x40 + ((c & 0x07) << 3)); //Set CG-RAM address
+                                           //8 sequential locations needed per UDC
+  // Store UDC pattern 
+  for (int i=0; i<8; i++) {
+    _writeData(*udc_data++);
+  }
+  
+  //Select DD RAM again for current LCD controller
+  addr = getAddress(_column, _row);
+  _writeCommand(0x80 | addr);
+  
+}
 
+#endif