OK, so I got it to work. I the registers in the datasheet are either wrong or I have the wrong datasheet, but here is what I figured out:
(1) It's really a 2x40 lcd, but the lines are cut in two so you have a 4x20
(2)
According to my datasheet the registers for each line should start at :
Line 1 : 0x0
Line 2 : 0x40
Line 3 : 0x14
Line 4 : 0x54
but the registers actually are :
Line 1 : 0x0
Line 2 : 0x28
Line 3 : 0x14
Line 4 : 0x3C
Weird ??
So I initialized the lcd as a two line lcd and I changed the cursorTo function to the following :
void LCD4Bit::cursorTo(int line_num, int x){
//first, put cursor home
commandWrite(CMD_HOME);
//if we are on a 1-line display, set line_num to 1st line, regardless of given
if (g_num_lines==1){
line_num = 1;
}
//offset 40 chars in if second line requested
if (line_num == 2){
x += 0x28;
}
if (line_num == 3){
x += 0x14;
}
if (line_num == 4){
x += 0x3C;
}
//advance the cursor to the right according to position. (second line starts at position 40).
for (int i=0; i<x; i++) {
commandWrite(0x14);
}
}
Now it finally works

