               }
               else{
                           // for all motors, unformated data
-            int index;
-            for(index = 0; index < motors_control.num_motors; index++) {
-              Serial.print("Motor: ");
-              Serial.println(index);
-              for (byte j = 0 ; j < motors_control.sensors[index].count ; ++j) {
-                byte sensor_index = motors_control.sensors[index].sensors_array[j].index;
-                byte type = motors_control.sensors[index].sensors_array[j].type;
-                Serial.print("Sensor index: ");
-                Serial.print(sensor_index);
-                Serial.print(", Type: ");
+                int index;
+                for(index = 0; index < motors_control.num_motors; index++) {
+                  Serial.write("Motor: ");
+                  Serial.println(index);
+                  for (byte j = 0 ; j < motors_control.sensors[index].count ; ++j) {
+                    byte sensor_index = motors_control.sensors[index].sensors_array[j].index;
+                    byte type = motors_control.sensors[index].sensors_array[j].type;
+                    Serial.write("Sensor index: ");
+                    Serial.print(sensor_index);
+                    Serial.write(", Type: ");
                 
-                if (POTENTIOMETER == type)
-                  Serial.print("potentiometer, Value: ");
-                  Serial.print(potentiometers_control.get_position(sensor_index));
-                  Serial.print(", Is whitin limits: ");
-                  Serial.println(potentiometers_control.isWithinLimits(sensor_index));
+                    if (POTENTIOMETER == type)
+                      Serial.write("potentiometer, Value: ");
+                      Serial.print(potentiometers_control.get_position(sensor_index));
+                      Serial.write(", Is whitin limits: ");
+                      Serial.println(potentiometers_control.isWithinLimits(sensor_index));
+                    }
                 }
-            }
-            i++;
-                        }
-        }
+                i++;
+              }
+        }
+        else
+         if (tmp_str[i] == 'T' || tmp_str[i] == 't'){// test connection
+           Serial.write("T#");
+         }
     }
     else
       i++;

  Serial.println(F("Motor index is between 0 and num_motors - 1"));
  
  Serial.println();

  current_buffer[0] = 0;
}

//--------------------------------------------------------------------------------------------

{
  motors_control.reset_pins();

current_buffer[0] = 0;

  Serial.begin(9600); //Open Serial connection
  
  Serial.write("Jenny 5 firmware version: ");
  Serial.write(firmware_version);
  Serial.write('\n');
  Serial.write("##");// initialization is over; must check for ## string

  /*
  Serial.write("Commands are:");
  Serial.println(F("T# // test connection. Returns T#."));
  Serial.println(F("Mx y# // Moves motor x with y steps. If y is negative the motor runs in the opposite direction. The motor remains locked at the end of the movement. Outputs Mx d# when motor rotation is over. If movement was complete, then d is 0, otherwise is the distance to go."));
  Serial.println(F("Dx#  // Disables motor x."));
  Serial.println(F("Lx#  // Lock motor x. Outputs Dx#"));
  Serial.println(F("SMx s a# // Sets speed of motor x to s and the acceleration to a."));
  Serial.println(F("SPx min max home# // Sets the parameters of a potentiometer. Min and max are the limits where it can move and home is from where we bring the robot when we start."));
  Serial.println(F("Ax n Py Bz ... # // Attach to motor x a list of n sensors (like Potentiometer y, Button z etc)."));
