diff --git a/Makefile b/Makefile
index 902a414..b28046c 100644
--- a/Makefile
+++ b/Makefile
@@ -11,8 +11,8 @@ TOOL    = stk500 -P /dev/ttyUSB0
 
 BUILDDIR = Builds
 
-DEFINES = -I . -IInternet/MQTT -I Internet/MQTT/MQTTPacket/src -I Ethernet/W5500 -I Ethernet -DF_CPU=16000000UL -D_WIZCHIP_=W5100
- 
+DEFINES = -DF_CPU=16000000UL
+
 CFLAGS  =-mmcu=$(MCU) -O2 -Wall -Wpedantic $(DEFINES) -std=c99 -ffunction-sections -fdata-sections
 LDFLAGS =-mmcu=$(MCU) -Wl,--gc-sections
 
diff --git a/main.c b/main.c
index ed362e6..44e15e6 100644
--- a/main.c
+++ b/main.c
@@ -3,6 +3,8 @@
 #include <stdint.h>
 #include <util/delay.h>
 
+#include "yaMBSiavr.h"
+
 uint8_t font[] = {
     0xEE,
     0x82,
@@ -17,6 +19,7 @@ uint8_t font[] = {
 };
 
 uint8_t display[4];
+volatile uint16_t holdingRegisters[4];
 
 void timer0_init()
 {
@@ -26,21 +29,71 @@ void timer0_init()
     TIMSK |= 1<<OCIE0;     //IRQ on TIMER0 output compareA
 }
 
+void timer2_init(void) {
+    //TCCR2 = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
+    //TCCR2 = (1<<CS21)|(1<<CS20); // PS 1:64
+    //OCR2 = 25; // 1ms reach for clear (16mz:64=>250kHz:250-=>1kHz)
+    //TIMSK |= 1<<OCIE2;     //IRQ on TIMER0 output compareA
+
+	TCCR2|=(1<<CS21); //prescaler 8
+	TIMSK|=(1<<TOIE2);
+
+}
+
+unsigned long bin2bcd (uint32_t n)     // convert 16 bit binary n to 5 digit BCD (packed)
+{
+    return n + 6 * (n/10 + 16*(n/100) + 256*(n/1000) + 4096*(n/10000));
+}
+
+void modbusGet(void) {
+    if (modbusGetBusState() & (1<<ReceiveCompleted))
+    {
+        switch(rxbuffer[1]) {
+            case fcReadHoldingRegisters:
+                modbusExchangeRegisters(holdingRegisters,0,4);
+                break;
+            case fcPresetMultipleRegisters:
+                modbusExchangeRegisters(holdingRegisters,0,1);
+
+                uint8_t h = holdingRegisters[0] / 3600;
+                uint8_t m = (holdingRegisters[0] / 60) % 60;
+                uint8_t s = holdingRegisters[0] % 60;
+
+                uint16_t bcd;
+                if(h)
+                    bcd = bin2bcd(m) | bin2bcd(h)<<8;
+                else
+                    bcd = bin2bcd(s) | bin2bcd(m)<<8;
+
+                display[0] = font[(bcd&0x000f)];
+                display[1] = font[(bcd&0x00f0) >> 4];
+
+                display[2] = font[(bcd&0x0f00) >> 8];
+                display[3] = bcd&0xF000 ? font[(bcd&0xf000) >> 12] : 0;
+                break;
+            default:
+                modbusSendException(ecIllegalFunction);
+                break;
+        }
+    }
+}
+
 int main(void){
-    DDRB |= _BV(3) | _BV(2) | _BV(1) | _BV(0);
+    DDRB |= _BV(4) | _BV(3) | _BV(2) | _BV(1) | _BV(0);
     DDRD = 0xFC;
     DDRA = 0x03 << 4;
     DDRC = 0xFE;
 
-    display[3] = font[1];
-    display[2] = font[3];
-    display[1] = font[3];
-    display[0] = font[7];
+	modbusSetAddress(24); //better set this to sth.
+	modbusInit();
 
     timer0_init();
+    timer2_init();
+
     sei();
 
     while(1){
+        modbusGet();
     }
 
 }
@@ -48,15 +101,19 @@ int main(void){
 ISR(TIMER0_COMP_vect)                             
 {
     if(PORTB & 0x01){
-        PORTB = 1<<1 | 1<<3;
+        PORTB = (PORTB & 0xF0) | 1<<1 | 1<<3;
         PORTD = display[3] & 0xFC;
-        PORTA = (display[3] & 0x03) << 4;
+        PORTA = (PORTA & 0xCF) | (display[3] & 0x03) << 4;
         PORTC = display[1];
     }
     else{
-        PORTB = 1<<0 | 1<<2;
+        PORTB = (PORTB & 0xF0) | 1<<0 | 1<<2;
         PORTD = display[2] & 0xFC;
-        PORTA = (display[2] & 0x03) << 4;
+        PORTA = (PORTA & 0xCF) | (display[2] & 0x03) << 4;
         PORTC = display[0];
     }
 }
+
+ISR(TIMER2_OVF_vect) { //this ISR is called 9765.625 times per second
+	modbusTickTimer();
+}