Updated for more display types. Fixed memoryaddress confusion in address() method. Added new getAddress() method. Added support for UDCs, Backlight control and other features such as control through I2C and SPI port expanders and controllers with native I2C and SPI interfaces. Refactored to fix issue with pins that are default declared as NC.

Dependents:   GPSDevice TestTextLCD SD to Flash Data Transfer DrumMachine ... more

Fork of TextLCD by Simon Ford

Example

Hello World! for the TextLCD

#include "mbed.h"
#include "TextLCD.h"
 
// Host PC Communication channels
Serial pc(USBTX, USBRX); // tx, rx
 
// I2C Communication
I2C i2c_lcd(p28,p27); // SDA, SCL
 
// SPI Communication
SPI spi_lcd(p5, NC, p7); // MOSI, MISO, SCLK

//TextLCD lcd(p15, p16, p17, p18, p19, p20);                // RS, E, D4-D7, LCDType=LCD16x2, BL=NC, E2=NC, LCDTCtrl=HD44780
//TextLCD_SPI lcd(&spi_lcd, p8, TextLCD::LCD40x4);   // SPI bus, 74595 expander, CS pin, LCD Type  
TextLCD_I2C lcd(&i2c_lcd, 0x42, TextLCD::LCD20x4);  // I2C bus, PCF8574 Slaveaddress, LCD Type
//TextLCD_I2C lcd(&i2c_lcd, 0x42, TextLCD::LCD16x2, TextLCD::WS0010); // I2C bus, PCF8574 Slaveaddress, LCD Type, Device Type
//TextLCD_SPI_N lcd(&spi_lcd, p8, p9);               // SPI bus, CS pin, RS pin, LCDType=LCD16x2, BL=NC, LCDTCtrl=ST7032_3V3   
//TextLCD_I2C_N lcd(&i2c_lcd, ST7032_SA, TextLCD::LCD16x2, NC, TextLCD::ST7032_3V3); // I2C bus, Slaveaddress, LCD Type, BL=NC, LCDTCtrl=ST7032_3V3  

int main() {
    pc.printf("LCD Test. Columns=%d, Rows=%d\n\r", lcd.columns(), lcd.rows());
    
    for (int row=0; row<lcd.rows(); row++) {
      int col=0;
      
      pc.printf("MemAddr(Col=%d, Row=%d)=0x%02X\n\r", col, row, lcd.getAddress(col, row));      
//      lcd.putc('-');
      lcd.putc('0' + row);      
      
      for (col=1; col<lcd.columns()-1; col++) {    
        lcd.putc('*');
      }
 
      pc.printf("MemAddr(Col=%d, Row=%d)=0x%02X\n\r", col, row, lcd.getAddress(col, row));      
      lcd.putc('+');
        
    }    
    
// Show cursor as blinking character
    lcd.setCursor(TextLCD::CurOff_BlkOn);
 
// Set and show user defined characters. A maximum of 8 UDCs are supported by the HD44780.
// They are defined by a 5x7 bitpattern. 
    lcd.setUDC(0, (char *) udc_0);  // Show |>
    lcd.putc(0);    
    lcd.setUDC(1, (char *) udc_1);  // Show <|
    lcd.putc(1);    

}

Handbook page

More info is here

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