diff --git a/lcdLib.c b/lcdLib.c
index d0989f9..7457b59 100644
--- a/lcdLib.c
+++ b/lcdLib.c
@@ -255,42 +255,40 @@ void writeCharToLCD(char c) {
   the end of the functions execution, the found_num uint8_t pointer will indicate how many
   digits were read.
  */
-uint8_t readASCIINumber(const char* str, uint8_t* found_num) {
-  const uint8_t num_digits = 4;
-  uint8_t nums[num_digits];
+uint8_t readASCIINumber(char* str, uint8_t* found_num, char** new_loc) {
+  uint8_t nums[3];
 
-  while (*str != '\0' && *found_num < num_digits - 1) {
+  *found_num = 0;
+  while (*str != '\0' && *found_num < 3) {
     if (*str >= 0x30 && *str <= 0x39) {
       // Use *str as a number (specified in ASCII)
-      nums[*(found_num++)] = *str - 0x30;
+      nums[(*found_num)++] = *str - 0x30;
     } else {
       break;
     }
 
     str++;
   }
+  *new_loc = str;
 
-  uint8_t ret = 0;
-  for (uint8_t i = 0; i < num_digits; i++)
-    ret += nums[num_digits - i] * pow(10, i);
-  return ret;
+  return nums[0]*100 + nums[1]*10 + nums[2];
 }
 
-void writeStringToLCD(const char* str) {
+void writeStringToLCD(char* str) {
   while (*str != '\0') {
     // Check for ANSI CSI (Control Sequence Introducer)
     if (*str == '\e') {
       if (*(++str) != '\0' && *str == '[') {
         // Read optional variable length number in ASCII (0x30 - 0x3f) where 0x3a - 0x3f are
-        // ignored (they are used as flags by some terminals
+        // ignored (they are used as flags by some terminals)
         uint8_t fnd0;
-        uint8_t num0 = readASCIINumber(str++, &fnd0);
+        uint8_t num0 = readASCIINumber(++str, &fnd0, &str);
 
         // Read optional (semicolon followed by optional variable length number)
         uint8_t fnd1;
         uint8_t num1;
-        if (*(++str) != '\0' && *str == ';') {
-          num1 = readASCIINumber(str++, &fnd1);
+        if (*str != '\0' && *str == ';') {
+          num1 = readASCIINumber(str, &fnd1, &str);
 
           // Read control character (between 0x40 - 0x7e) for two argument sequences
           switch (*str) {
@@ -314,6 +312,7 @@ void writeStringToLCD(const char* str) {
             break;
             ;;
           case 'B': // CUD - Cursor down
+            PORTC ^= (1 << PC5);
             num0 = fnd0 ? num0 : 1;
             moveCursorDown(num0);
             break;
@@ -344,6 +343,7 @@ void writeStringToLCD(const char* str) {
             break;
             ;;
           default:  // Invalid control character
+            writeCharToLCD(*str);
             break;
             ;;
           }
@@ -407,7 +407,7 @@ void moveCursorUp(uint8_t n) {
 }
 
 void moveCursorDown(uint8_t n) {
-  if (n + currentLineNum > LCD_NUMBER_OF_LINES - 1) {
+  if (n + currentLineNum < LCD_NUMBER_OF_LINES + 1) {
     currentLineNum += n;
   } else {
     currentLineNum = LCD_NUMBER_OF_LINES - 1;
@@ -417,7 +417,7 @@ void moveCursorDown(uint8_t n) {
 }
 
 void moveCursorForward(uint8_t n) {
-  if (n + currentLineChars > LCD_CHARACTERS_PER_LINE - 1) {
+  if (n + currentLineChars < LCD_CHARACTERS_PER_LINE + 1) {
     currentLineChars += n;
   } else {
     currentLineChars = LCD_CHARACTERS_PER_LINE - 1;
@@ -428,7 +428,7 @@ void moveCursorForward(uint8_t n) {
 
 void moveCursorBackward(uint8_t n) {
   if (n < currentLineChars + 1) {
-    currentLineChars += n;
+    currentLineChars -= n;
   } else {
     currentLineChars = 0;
   }
diff --git a/lcdLib.h b/lcdLib.h
index 1d873af..dea3256 100644
--- a/lcdLib.h
+++ b/lcdLib.h
@@ -137,7 +137,7 @@ void writeCharToLCD(char);
 /**
   Writes a string to the LCD starting from the current cursor position.
  */
-void writeStringToLCD(const char*);
+void writeStringToLCD(char*);
 
 //-----------------------------------------------------------------------------------------------
 // LCD command functions
diff --git a/lcdOutput.c b/lcdOutput.c
index 63421a1..dd6193b 100644
--- a/lcdOutput.c
+++ b/lcdOutput.c
@@ -78,44 +78,7 @@ int main(void) {
   //initLCDByInternalReset();
   flashLED(5); // DEBUG
 
-
-  writeStringToLCD("Backwards 4" CUB(4));
-  _delay_ms(5000);
-
-  clearDisplay();
-  writeStringToLCD("Backwards 3" CUB(3));
-  _delay_ms(5000);
-
-    clearDisplay();
-  writeStringToLCD("Backwards 2" CUB(2));
-  _delay_ms(5000);
-
-  clearDisplay();
-  writeStringToLCD("Backwards 1" CUB(1));
-  _delay_ms(5000);
-
-  clearDisplay();
-  writeStringToLCD("Forwards 4" CUF(4));
-  _delay_ms(5000);
-
-  /* writeStringToLCD(CNL(1)); */
-  /* _delay_ms(5000); */
-
-  /* writeStringToLCD(CUF(10)); */
-  /* _delay_ms(5000); */
-
-  /* writeStringToLCD(CPL(1)); */
-  /* _delay_ms(5000); */
-
-  /* writeStringToLCD(CHA(20)); */
-  /* _delay_ms(5000); */
-
-  /* writeStringToLCD(CUP(20,20)); */
-  /* _delay_ms(5000); */
-
-  /* writeStringToLCD(CUP(1,1)); */
-  /* _delay_ms(5000); */
-
+  writeStringToLCD(CUB(1));
 
   while (1) {
     serialChar = receiveByte();
@@ -125,21 +88,38 @@ int main(void) {
       writeStringToLCD("\r\n");
       transmitString("\n" CNL(1) "\r");
       break;
-      ;
+      ;;
     case '\f':
       writeCharToLCD(serialChar);
       transmitString(ED(2) CUP(1,1));
       break;
-      ;
+      ;;
     case 0x7f: // Backspace (sent as delete)
       writeStringToLCD("\b \b");
       transmitString(CUB(1) " " CUB(1));
       break;
-      ;
+      ;;
+    case '\e': // Beginning of ANSI escape
+      {
+        char j = receiveByte();
+
+        if (j == '[') {
+          char buf[7] = "\e[";
+          for (uint8_t i = 2, j = receiveByte(); i < 7 && j > 0x20 && j < 0x7e; i++, j = receiveByte()) {
+            buf[i] = j;
+            if (j > 0x40 && j < 0x7e) {
+              break;
+            }
+          }
+          writeStringToLCD(buf);
+        }
+        break;
+      }
+      ;;
     default:
       writeCharToLCD(serialChar);
       transmitByte(serialChar);   // Echo character back to serial console
-      ;
+      ;;
     }
   }