diff --git a/Laser_Test/General.ino b/Laser_Test/General.ino
index c153193..3d2ae49 100644
--- a/Laser_Test/General.ino
+++ b/Laser_Test/General.ino
@@ -1,23 +1,33 @@
 
-//Generate line1 and line 2 with 14 chars
-void Generate_Code14()
+//Display
+void dsp_info(char *buf, int line)
+{
+
+  display.setCursor(0, (line - 1) * 10);
+  display.println(F(buf));
+  display.display();
+}
+
+
+//Scroll line
+void Scroll_Line(char *scrol_line, int char_count, bool scrol_direction )
 {
   byte tmp_byte;
 
-  //scroll chars line 1
-  tmp_byte = line1_14[13];
-  for (int i = 12; i >= 0; i--)
-  {
-    line1_14[i + 1] = line1_14[i];
+  if (!scrol_direction) {
+    //scroll chars line left to right
+    tmp_byte = scrol_line[char_count - 1];
+    for (int i = (char_count - 2); i >= 0; i--) {
+      scrol_line[i + 1] = scrol_line[i];
+    }
+    scrol_line[0] = tmp_byte;
   }
-  line1_14[0] = tmp_byte;
-
-
-  //scroll chars line 2
-  tmp_byte = line2_14[0];
-  for (int i = 1; i <= 13; i++)
-  {
-    line2_14[i - 1] = line2_14[i];
+  else {
+    //scroll chars line right to left
+    tmp_byte = scrol_line[0];
+    for (int i = 1; i <= (char_count - 1); i++) {
+      scrol_line[i - 1] = scrol_line[i];
+    }
+    scrol_line[char_count - 1] = tmp_byte;
   }
-  line2_14[13] = tmp_byte;
 }
diff --git a/Laser_Test/Laser_Test.ino b/Laser_Test/Laser_Test.ino
index 299bd30..c5dbfdc 100644
--- a/Laser_Test/Laser_Test.ino
+++ b/Laser_Test/Laser_Test.ino
@@ -27,8 +27,6 @@ int incomingByte = 0;                 // for incoming serial data
 bool b_mode;
 bool b_type;
 int lasertype = 0;                    //Lasertype selected
-//tbv 500SL
-bool watchdog_send = false;           //500SL Wachtdog send, thus switched to vectorservice protocol
 
 //void Linx500SL_Watchdog();
 //void Linx500SL_SendCode();
@@ -36,25 +34,22 @@ bool watchdog_send = false;           //500SL Wachtdog send, thus switched to ve
 char line1_14[] = {'1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '1', '2', '3', '4'};
 char line2_14[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N'};
 
-void setup() {
-
-
+void setup()
+{
   pinMode(pin_LASERTYPE, INPUT_PULLUP);
   pinMode(pin_MODE, INPUT_PULLUP);
 
   //init debug port
   Serial.begin(9600);
 
-  if (digitalRead(pin_LASERTYPE) == false)
-  {
+  if (digitalRead(pin_LASERTYPE) == false) {
     Serial.println("Lasertype set to : Linx 500SL - baudrate 19k2");
     //init serial ports
     MySerial1.begin(19200, SERIAL_8N1, pin_RXD1, pin_TXD1);
     MySerial2.begin(19200, SERIAL_8N1, pin_RXD1, pin_TXD2);
     lasertype = 1;
   }
-  else
-  {
+  else {
     Serial.println("Lasertype set to : Linx ??? - baudrate 9600");
     //init serial ports
     MySerial1.begin(9600, SERIAL_8N1, pin_RXD1, pin_TXD1);
@@ -71,75 +66,47 @@ void setup() {
   // Clear the buffer
   display.clearDisplay();
   display.setTextSize(1);
-  display.setTextColor(SSD1306_WHITE);
+  //display.setTextColor(SSD1306_WHITE);
+  display.setTextColor(WHITE, BLACK);
+
   display.setCursor(10, 0);
-  display.println(F("RS232 Sniffer v1.0"));
+  display.println(F("RS232 Sniffer v1.1"));
   delay(1000);
   display.display();
   display.clearDisplay();
   display.setCursor(10, 0);
   if (lasertype == 1) {
-    display.println(F("Linx 500SL"));
+    dsp_info("Linx 500SL     ", 1);
   }
   if (lasertype == 2) {
-    display.println(F("MundiScan 2000"));
+    dsp_info("MundiScan 2000", 1);
   }
-  display.setCursor(0, 10);
-  display.println(F("Sniffer mode selected"));
-  display.display();
-  delay(2000);
 
+  dsp_info("Sniffer Mode   ", 2);
+  delay(2000);
 }
 
 
-void loop() {
+void loop()
+{
   // put your main code here, to run repeatedly:
 
-  if (digitalRead(pin_MODE) != b_mode)
-  {
+  if (digitalRead(pin_MODE) != b_mode) {
     b_mode = digitalRead(pin_MODE);
-    if (b_mode)
-    {
+    if (b_mode) {
       Serial.println("Mode : Sniffer");
-      display.clearDisplay();
-      display.setTextSize(1);
-      display.setTextColor(SSD1306_WHITE);
-      display.setCursor(10, 0);
-      if (lasertype == 1) {
-        display.println(F("Linx 500SL"));
-      }
-      if (lasertype == 2) {
-        display.println(F("MundiScan 2000"));
-      }
-      display.setCursor(0, 10);
-      display.println(F("Sniffer mode selected"));
-      display.display();
+      dsp_info("Sniffer Mode    ", 2);
       delay(1000);
     }
-    else
-    {
+    else {
       Serial.println("Mode : Generate code");
-      display.clearDisplay();
-      display.setTextSize(1);
-      display.setTextColor(SSD1306_WHITE);
-      display.setCursor(10, 0);
-      if (lasertype == 1) {
-        display.println(F("Linx 500SL"));
-      }
-      if (lasertype == 2) {
-        display.println(F("MundiScan 2000"));
-      }
-      display.setCursor(0, 10);
-      display.println(F("Send mode selected"));
-      display.display();
+      dsp_info("Generate Code    ", 2);
+
       delay(1000);
     }
-
-
   }
 
-  if (b_mode)
-  {
+  if (b_mode) {
     if (MySerial1.available() > 0) {
       // read the incoming byte:
       incomingByte = MySerial1.read();
@@ -160,55 +127,15 @@ void loop() {
   }
 
   //generating mode
-  else
-  {
+  else {
     now = millis();
-    if (now - prev >= 5000)
-    {
-
+    if (now - prev >= 5000) {
       //Laser 500SL
-      if (lasertype == 1)
-      {
-        if (!watchdog_send)
-        {
-          display.setCursor(0, 20);
-
-          display.setTextColor(WHITE, BLACK);
-
-          display.println(F("Sending WATCHDOG       "));
-          display.display();
-
-          Serial.println("Linx 500SL : Sending WatchDog command to switch to VectorService Protocol");
-
-          Linx500SL_Watchdog();
-          if (!watchdog_send) {
-            Serial.println("Linx 500SL : Error !! --> No Response from laser");
-          }
-          if (watchdog_send) {
-            Serial.println("Linx 500SL : Laser switched to VectorService Protocol !!");
-          }
-
-          display.setCursor(0, 20);
-          display.setTextColor(WHITE, BLACK);
-          if (!watchdog_send) {
-            display.println(F("No Response laser !! "));
-          }
-          if (watchdog_send) {
-            display.println(F("Laser Connected !!  "));
-          }
-          display.display();
-          delay(5000);
-          // ik even geen laser heb ;-)
-          watchdog_send = true;
-        }
-        else
-        {
-          Linx500SL_SendCode();
-        }
+      if (lasertype == 1) {
+        SL500_generate_code();
       }
 
-      if (lasertype == 2)
-      {
+      if (lasertype == 2) {
         Serial.println(".");
         MySerial1.println("P1 : Hallo Frank");
         MySerial2.println("P2 : Hallo Frank");
@@ -216,8 +143,5 @@ void loop() {
 
       prev = now;
     }
-
-
-
   }
 }
diff --git a/Laser_Test/Linx500SL.ino b/Laser_Test/Linx500SL.ino
index 8d83728..8ff75d2 100644
--- a/Laser_Test/Linx500SL.ino
+++ b/Laser_Test/Linx500SL.ino
@@ -1,12 +1,48 @@
+//tbv 500SL
+#define pos_seq 16
+#define pos_txt 25
+
+bool SL500_watchdog_send = false;           //500SL Wachtdog send, thus switched to vectorservice protocol
 
 byte SL500_SEQ = 00;
 byte SL500_CRC = 00;
 byte SL500_XOR = 00;
 
+void SL500_generate_code()
+{
+  //First check if laser is in VectorSevice Protocol
+  if (!SL500_watchdog_send) {
+    dsp_info("Sending WATCHDOG     ",3);
+    Serial.println("Linx 500SL : Sending WatchDog command to switch to VectorService Protocol");
+
+    Linx500SL_Watchdog();
+    if (!SL500_watchdog_send) {
+      Serial.println("Linx 500SL : Error !! --> No Response from laser");
+    }
+    if (SL500_watchdog_send) {
+      Serial.println("Linx 500SL : Laser switched to VectorService Protocol !!");
+    }
+
+    if (!SL500_watchdog_send) {
+      dsp_info("No Response laser !!   ",3);
+    }
+    if (SL500_watchdog_send) {
+      dsp_info("Laser Connected !!  ",3);
+    }
+
+    delay(5000);
+    
+    // ik even geen laser heb ;-)
+    SL500_watchdog_send = true;
+  }
+  else {
+    Linx500SL_SendCode();
+  }
+}
+
+
+
 //Send Wachtdog and wait for a reply to switch 500SL laser into VectorService Protocol
-
-//Wachdog command CC AA 00 00 00 01 00 00 00 01 00 13 00 00 00 2B [SEQ] [CRC] [XOR]
-
 void Linx500SL_Watchdog()
 {
   char SL500_WD[] = {0xCC, 0xAA, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x13, 0x00, 0x00, 0x00, 0x2B, 0x00, 0x00, 0x00};
@@ -14,9 +50,7 @@ void Linx500SL_Watchdog()
   int count = 0;
   byte p2_rec_byte;
 
-  while ((!watchdog_send) && count < 5)
-  {
-
+  while ((!SL500_watchdog_send) && count < 5) {
     SL500_SEQ++;
     SL500_WD[16] = SL500_SEQ;
 
@@ -25,11 +59,9 @@ void Linx500SL_Watchdog()
     SL500_WD[17] = SL500_CRC;
     SL500_WD[18] = SL500_XOR;
 
-
     Serial.println("Linx 500SL : WatchDog");
     //Send karakters of the watchdog command
-    for (int i = 0; i <= 18; i++)
-    {
+    for (int i = 0; i <= 18; i++) {
       //     MySerial1.print(SL500_WD[i], DEC);
       MySerial1.write(SL500_WD[i]);
     }
@@ -37,15 +69,13 @@ void Linx500SL_Watchdog()
     //Small delay to give the laser some time to respond
     delay(500);
 
-    if (MySerial1.available() > 0)
-    {
+    if (MySerial1.available() > 0) {
       // read the incoming byte:
       p2_rec_byte = MySerial1.read();
     }
 
-    if (p2_rec_byte == 204)
-    {
-      watchdog_send = true;
+    if (p2_rec_byte == 0xCC) {
+      SL500_watchdog_send = true;
     }
     count = count + 1;
     delay(2000);
@@ -63,18 +93,21 @@ void Linx500SL_SendCode()
   //pos 20 = Remote update Field
   //pos 16 = SEQ
   //pos 25 = Start tekst
-  #define pos_seq 16
-  #define pos_txt 25
-  
-  Generate_Code14();
+
+  //Generate_Code14();
 
   //Line 1
+  Scroll_Line(line1_14, 14, 0);
   SL500_SEQ++;
   line1[pos_seq] = SL500_SEQ;
+  
+//  display.setCursor(0, 20);
+//  display.println(F("No Response laser !! "));
+
+  dsp_info("Send line 1 to laser",3);
   Serial.println("");
   Serial.print("Linx500SL --> Line 1 : ");
-  for (int i = 0; i <= 13; i++)
-  {
+  for (int i = 0; i <= 13; i++) {
     line1[pos_txt + (i * 2)] = line1_14[i];
     line1[pos_txt + (i * 2) + 1] = 0;
     Serial.write(line1_14[i]);
@@ -86,22 +119,26 @@ void Linx500SL_SendCode()
   line1[pos_txt + 28 + 1] = SL500_XOR;
 
   //Write all chars to Serial port
-  for (int i = 0; i <= (pos_txt + 28 + 1); i++)
-  {
+  for (int i = 0; i <= (pos_txt + 28 + 1); i++) {
     MySerial1.write(line1[i]);
   }
   Serial.println("");
 
+  delay(1000);
+  
   //Line 2
+  Scroll_Line(line2_14, 14, 1);
   SL500_SEQ++;
   line2[pos_seq] = SL500_SEQ;
+  
+//  dsp_info(line2_14,3);
+  dsp_info("Send line 2 to laser",3);
   Serial.println("");
   Serial.print("Linx500SL --> Line 2 : ");
-  for (int i = 0; i <= 13; i++)
-  {
+  for (int i = 0; i <= 13; i++) {
     line2[pos_txt + (i * 2)] = line2_14[i];
     line2[pos_txt + (i * 2) + 1] = 0;
-    Serial.write(line1_14[i]);
+    Serial.write(line2_14[i]);
   }
 
   Linx500_Checksum(line2, pos_txt + 28);
@@ -110,12 +147,10 @@ void Linx500SL_SendCode()
   line2[pos_txt + 28 + 1] = SL500_XOR;
 
   //Write all chars to Serial port
-  for (int i = 0; i <= (pos_txt + 28 + 1); i++)
-  {
+  for (int i = 0; i <= (pos_txt + 28 + 1); i++) {
     MySerial1.write(line2[i]);
   }
   Serial.println("");
-
 }
 
 
@@ -126,8 +161,7 @@ void Linx500_Checksum(char *cmd_line, int char_count)
   long val_xor = 0;
 
   //Checksum Calculation
-  for (int i = 0; i <= char_count; i++)
-  {
+  for (int i = 0; i <= char_count; i++) {
     val_crc = val_crc + cmd_line[i];
     val_xor = val_xor ^ cmd_line[i];
   }