   Ethernet.begin(mac, ip);
   server.begin();
 
-  Serial.print("Ethernet initialized. IP: ");
-  Serial.println(Ethernet.localIP());
-}
-
+  DEBUG_PRINT("Ethernet initialized. IP: ");
+  Serial.print(Ethernet.localIP());
+  Serial.print(":");
+  Serial.println(TCP_PORT);  
+}
+
+#ifdef DEBUG
 void writeTime() {
   Serial.print("Time elapsed = ");
   Serial.println(millis() - time);
 }
+#endif
 
 // Read data from DHT sensor
 void readDHT() {
-  writeTime();
-  Serial.println("Reading from DHT sensor");
-  float h = dht.readHumidity();
-  float t = dht.readTemperature(); // *C
-  Serial.println("Done");
-  writeTime();
-
-  if (isnan(h) || isnan(t)) {
-    Serial.println("Error reading from DHT sensor");
+  DEBUG_WRITETIME();
+  DEBUG_PRINTLN("Reading from DHT sensor");
+  
+  dhtH = dht.readHumidity();
+  dhtT = dht.readTemperature(); // *C
+  
+  DEBUG_WRITETIME();
+
+  if (isnan(dhtH) || isnan(dhtT)) {
+    dhtH = EMPTY_VAL;
+    dhtT = EMPTY_VAL;
+    DEBUG_PRINTLN("Error reading from DHT sensor");
   } else {
-    Serial.print("h = ");
-    Serial.print(h);
-    Serial.println(" %");
-    Serial.print("t = ");
-    Serial.print(t);
-    Serial.println(" *C");
-  }
-  writeTime();
-  Serial.println();
-}
-
-// Read data from DS sensors
-void readDS() {
-  writeTime();
-  Serial.println("Requesting DS sensors");
-  for (int i = 0; i < ONE_WIRE_CNT; i++) {
-    dsSensors[i].requestTemperatures();
-  }
-  Serial.println("Done");
-  writeTime();
-  
-  for (int i=0; i < ONE_WIRE_CNT; i++) {
-    float t = dsSensors[i].getTempCByIndex(0);
-    Serial.print("t[");
-    Serial.print(i);
-    Serial.print("] = ");
-    Serial.print(t);
-    Serial.println(" *C");
-  }  
-  writeTime();
-  Serial.println();
+    DEBUG_PRINT("h = ");
+    DEBUG_PRINTLN(dhtH);
+    DEBUG_PRINT("t = ");
+    DEBUG_PRINTLN(dhtT);
+  }
+  
+  DEBUG_WRITETIME();
+  DEBUG_PRINTLN();
+}
+
+// Read data from DS sensor
+void readDS(int index) {
+  DEBUG_WRITETIME();
+  DEBUG_PRINT("Requesting DS sensor ");
+  DEBUG_PRINTLN(index);
+
+  dsSensors[index].requestTemperatures();
+  
+  DEBUG_WRITETIME();
+  
+  dsT[index] = dsSensors[index].getTempCByIndex(0);
+
+  DEBUG_PRINT("t = ");
+  DEBUG_PRINTLN(dsT[index]);
+  DEBUG_WRITETIME();
+  DEBUG_PRINTLN();
 }
 
 // Process Ethernet communication
 void communicate() {
-  writeTime();
+  DEBUG_WRITETIME();
 
   // manage connection
   if (clientConnected) {
     if (client.connected()) {
-      Serial.println("Client is still connected");
+      DEBUG_PRINTLN("Client is still connected");
     } else {
       client.stop();
       clientConnected = false;
-      Serial.println("Client has been disconnected");
+      DEBUG_PRINTLN("Client has been disconnected");
     }
   }
 

  initDHT();
  initDS();
  initEthernet();
  Serial.println();
}

void loop() {
  time = millis();
  
  // wait between measurements
  delay(500);

  // read temperature and humidity from DHT sensors, takes around 270 milliseconds
  readDHT();
  
  // read temperatures from DS sensors, takes around 2370 milliseconds
  readDS();

  // Ethernet communication
  communicate();
}

OneWire oneWire2(ONE_WIRE_PIN2);
OneWire oneWires[] = {oneWire0, oneWire1, oneWire2};
DallasTemperature dsSensors[ONE_WIRE_CNT];
float dsT[ONE_WIRE_CNT] = { EMPTY_VAL, EMPTY_VAL, EMPTY_VAL };

// Ethernet
const byte mac[] = { 0xAB, 0xB6, 0xAA, 0x43, 0xA5, 0x9D };
IPAddress ip(192, 168, 1, 8);
EthernetServer server = EthernetServer(TCP_PORT);
EthernetClient client;
boolean clientConnected = false;
const byte modbusHeader[MODBUS_HDR_LEN] = { 
  0x00, 0x00, // Transaction identifier
  0x00, 0x00, // Protocol identifier
  0x00, 0x17, // Length
  0x00,       // Unit identifier
  0x03,       // Function code
  0x14        // Byte count (20)
};

// Speed control
#ifdef DEBUG
unsigned long time;
#endif

// Initialize DHT sensor
void initDHT() {
  dht.begin();
  DEBUG_PRINTLN("DHT sensor initialized");
}

// Initialize DS sensors
