diff --git a/Makefile b/Makefile
index e5279be..cb66d34 100644
--- a/Makefile
+++ b/Makefile
@@ -4,6 +4,7 @@ FILES   = $(SRCS:%.c=%) #main uart avrIOhelper/io-helper #uart#hier alle c-Datei
 MCU     = atmega2560
 PROGC	= m2560
 CC      = avr-gcc
+#TOOL    = stk500 -P /dev/ttyUSB0
 TOOL    = atmelice_isp
 #TOOL    = avrispmkii
 #TOOL    = usbasp-clone
diff --git a/avrIOhelper/io-helper.h b/avrIOhelper/io-helper.h
index 71071f1..313c787 100644
--- a/avrIOhelper/io-helper.h
+++ b/avrIOhelper/io-helper.h
@@ -68,13 +68,18 @@ void ioHelperEdgeDetector(void);
 #define BitPD6 22 //D22
 #define BitPJ4 23 //D23
 
+#define LED_AMPEL_ROT               BitPJ4
+#define LED_AMPEL_GELB              BitPD6
+#define LED_AMPEL_GRUEN             BitPD5
 
-#define LED_GRN_NOTAUS_ANLAGE   BitPE4
-#define LED_ROT_NOTAUS_ANLAGE   BitPE5
-#define LED_GRN_NOTAUS_SCHRANK  BitPG5
-#define LED_ROT_NOTAUS_SCHRANK  BitPE3
-#define LED_PLC_OK              BitPH5
-#define LED_BUS_OK              BitPD4
+#define LED_GRN_NOTAUS_SCHRANK  BitPB6
+#define LED_ROT_NOTAUS_SCHRANK  BitPB7
+#define LED_GRN_NOTAUS_ANLAGE   BitPL1
+#define LED_ROT_NOTAUS_ANLAGE   BitPL2
+#define LED_GRN_NOTAUS_ANLAGE_R   BitPL0
+#define LED_ROT_NOTAUS_ANLAGE_R   BitPD4
+#define LED_PLC_OK              BitPL5
+#define LED_BUS_OK              BitPL3
 
 
 //Inputs
@@ -90,11 +95,23 @@ void ioHelperEdgeDetector(void);
 
 #define BitPinK0 8  //A8
 #define BitPinK1 9  //A9
+#define BitPinK2 10 //A10
+#define BitPinK3 11 //A11
+#define BitPinK4 12 //A12
+#define BitPinK5 13 //A13
+#define BitPinK6 14 //A14
+#define BitPinK7 15 //A15
+
+#define BitPinD7 16 //I16
+#define BitPinG2 17 //I17
+#define BitPinG1 18 //I18
 #define BitPinD3 10 //INO
 #define BitPinD2 11 //IN1
 
-#define IN_ANLAGE_EIN_INV       BitPinF5
-#define IN_NOTAUS_ANLAGE        BitPinF7
-#define IN_NOTAUS_SCHRANK       BitPinK0
+
+#define IN_ANLAGE_EIN_INV           BitPinF7
+#define IN_NOTAUS_ANLAGE_R      BitPinK5
+#define IN_NOTAUS_ANLAGE        BitPinK4
+#define IN_NOTAUS_SCHRANK       BitPinK6
 
 #endif
diff --git a/kraftsensor.c b/kraftsensor.c
new file mode 100644
index 0000000..b105b5c
--- /dev/null
+++ b/kraftsensor.c
@@ -0,0 +1,53 @@
+#include <avr/io.h>
+#include <stdio.h>
+
+#include "kraftsensor.h"
+#include "modbus.h"
+#include "modbus-master.h"
+
+int32_t kraftsensor_value;
+uint8_t kraftsensor_valid;
+
+int32_t kraftsensor_zero_offset = 0/*197700*/;
+
+void do_kraftsensor(){
+    //static int32_t last_read;
+    //static int32_t old_value;
+    uint16_t m_data[4];
+    //int32_t kraftsensor_read;
+
+    /* read 2 16bit values and merge to 32bit signed integer */
+    readReg(1,0,2);
+    if(wait_receive(2, m_data, 10)){
+        kraftsensor_valid = 0;
+        printf("modbus error\n\r");
+    }
+    else{
+        kraftsensor_valid = 1;
+        int32_t tmp = (uint32_t)m_data[1]<<16;
+        tmp |= m_data[0];
+
+        //kraftsensor_value = tmp;
+        /* conversion magic to milliNewton */
+        kraftsensor_value = (((tmp /* + 539363*/)*9.81)/177.380)+kraftsensor_zero_offset;
+
+        //if(abs(kraftsensor_read - old_value) > 10000){
+        //    if(abs(last_read - kraftsensor_read) > 10000){
+        //        kraftsensor_value = old_value;
+        //        //printf("delta: %ld\tvalue:%ld\n", kraftsensor_read - old_value, kraftsensor_read);
+        //        //printf("spike\n");
+        //    }
+        //    else{
+        //        kraftsensor_value = kraftsensor_read;
+        //        //printf("jump\n");
+        //    }
+        //}
+        //else{
+        //    kraftsensor_value = kraftsensor_read;
+        //}
+        //last_read = kraftsensor_read;
+        //old_value = kraftsensor_value;
+    }
+
+}
+
diff --git a/kraftsensor.h b/kraftsensor.h
new file mode 100644
index 0000000..d0ef662
--- /dev/null
+++ b/kraftsensor.h
@@ -0,0 +1,15 @@
+#ifndef _MODBUS_MASTER_H_
+#define _MODBUS_MASTER_H_
+
+#include <stdint.h>
+
+#define receiveOkay (modbusGetBusState() & (1<<ReceiveCompleted))
+
+extern int32_t kraftsensor_value;
+extern uint8_t kraftsensor_valid;
+
+extern int32_t kraftsensor_zero_offset;
+
+void do_kraftsensor(void);
+
+#endif
diff --git a/main.c b/main.c
index 7ca309c..6f8b3f0 100644
--- a/main.c
+++ b/main.c
@@ -1,6 +1,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <avr/wdt.h> // WatchDog          
+#include <stdint.h>
 #include <string.h>
 
 #include "Ethernet/socket.h"
@@ -10,14 +11,17 @@
 #include "Internet/MQTT/MQTTClient.h"
 
 #include "avrIOhelper/io-helper.h"
+#include "kraftsensor.h"
 #include "millis.h"
 #include "uart.h"
 #include "spi.h"
 #include "mqtt.h"
+#include "modbus.h"
+#include "modbus-master.h"
 
 #include "util/delay.h"
 
-#define PLC_MQTT_ENABLED 0
+#define PLC_MQTT_ENABLED 1
 
 Client mqtt_client;
 
@@ -42,6 +46,8 @@ static void avr_init(void);
 void timer0_init(void);
 void print_network_information(void);
 
+volatile uint16_t ADC_reading = 0;
+
 void IO_LIBRARY_Init(void) {
     uint8_t bufSize[] = {2, 2, 2, 2, 2, 2, 2, 2};
 
@@ -55,41 +61,146 @@ void IO_LIBRARY_Init(void) {
 }
 
 void do_notaus(){
-    if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) || read_Input(IN_NOTAUS_SCHRANK, LEVEL)){   // NOTAUS
-            set_Output(LED_GRN_NOTAUS_SCHRANK, OFF);                                    // disable green lamps
-            set_Output(LED_GRN_NOTAUS_ANLAGE, OFF);
-    }
+    if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) || read_Input(IN_NOTAUS_SCHRANK, LEVEL) || read_Input(IN_NOTAUS_ANLAGE_R, LEVEL)){
+        /* at least one pressed */
+        //notaus_state = POWER_OFF;
+
+        ioHelperSetBit(outStates, LED_AMPEL_ROT,  1);
+        ioHelperSetBit(outStates, LED_AMPEL_GELB,  0);
+        ioHelperSetBit(outStates, LED_AMPEL_GRUEN,  0);
+
+        set_Output(LED_GRN_NOTAUS_SCHRANK,   OFF);
+        set_Output(LED_GRN_NOTAUS_ANLAGE,    OFF);
+        set_Output(LED_GRN_NOTAUS_ANLAGE_R,   OFF);
+
+        if(read_Input(IN_NOTAUS_ANLAGE, LEVEL)){
+            set_Output(LED_ROT_NOTAUS_ANLAGE, BLINK);
+            set_Output(LED_ROT_NOTAUS_SCHRANK,   ON);
+            set_Output(LED_ROT_NOTAUS_ANLAGE_R,   ON);
+        }
+
+        if(read_Input(IN_NOTAUS_SCHRANK, LEVEL)){
+            set_Output(LED_ROT_NOTAUS_ANLAGE,    ON);
+            set_Output(LED_ROT_NOTAUS_SCHRANK, BLINK);
+            set_Output(LED_ROT_NOTAUS_ANLAGE_R,   ON);
+        }
+
+        if(read_Input(IN_NOTAUS_ANLAGE_R, LEVEL)){
+            set_Output(LED_ROT_NOTAUS_ANLAGE,    ON);
+            set_Output(LED_ROT_NOTAUS_SCHRANK,   ON);
+            set_Output(LED_ROT_NOTAUS_ANLAGE_R, BLINK);
+        }
 
-    if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) && read_Input(IN_NOTAUS_SCHRANK, LEVEL)){   // both activated
-        set_Output(LED_ROT_NOTAUS_ANLAGE, BLINK);
-        set_Output(LED_ROT_NOTAUS_SCHRANK, BLINK);
     }
-    else if(read_Input(IN_NOTAUS_ANLAGE, LEVEL)){                                      // top one activated
-        set_Output(LED_ROT_NOTAUS_ANLAGE, BLINK);
+    else if(!read_Input(IN_ANLAGE_EIN_INV, LEVEL)){
+        /* nothing pressed, but power not on */
+        //notaus_state = ARMED;
+
+        ioHelperSetBit(outStates, LED_AMPEL_ROT,  0);
+        ioHelperSetBit(outStates, LED_AMPEL_GELB,  1);
+        ioHelperSetBit(outStates, LED_AMPEL_GRUEN,  0);
+
+        set_Output(LED_ROT_NOTAUS_ANLAGE,  ON);
         set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
+        set_Output(LED_ROT_NOTAUS_ANLAGE_R, ON);
+
+        set_Output(LED_GRN_NOTAUS_SCHRANK, ON);
+        set_Output(LED_GRN_NOTAUS_ANLAGE,  ON);
+        set_Output(LED_GRN_NOTAUS_ANLAGE_R, ON);
     }
-    else if(read_Input(IN_NOTAUS_SCHRANK, LEVEL)){                                     // bottom one activated
-        set_Output(LED_ROT_NOTAUS_SCHRANK, BLINK);
-        set_Output(LED_ROT_NOTAUS_ANLAGE, ON);
-    }
-    else{                                                                              // none activated
-            set_Output(LED_ROT_NOTAUS_SCHRANK, OFF);
-            set_Output(LED_ROT_NOTAUS_ANLAGE, OFF);
-            if(!read_Input(IN_ANLAGE_EIN_INV, LEVEL)){
-                set_Output(LED_GRN_NOTAUS_ANLAGE, ON);
-                set_Output(LED_GRN_NOTAUS_SCHRANK, ON);
-            }
-            else{
-                set_Output(LED_GRN_NOTAUS_ANLAGE, ON);
-                set_Output(LED_GRN_NOTAUS_SCHRANK, ON);
-                set_Output(LED_ROT_NOTAUS_ANLAGE, ON);
-                set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
-            }
+    else{
+        /* powered on */
+        //notaus_state = POWER_ON;
+
+        ioHelperSetBit(outStates, LED_AMPEL_ROT,  0);
+        ioHelperSetBit(outStates, LED_AMPEL_GELB,  0);
+        ioHelperSetBit(outStates, LED_AMPEL_GRUEN,  1);
+
+        set_Output(LED_GRN_NOTAUS_SCHRANK,   ON);
+        set_Output(LED_GRN_NOTAUS_ANLAGE,    ON);
+        set_Output(LED_GRN_NOTAUS_ANLAGE_R,   ON);
+
+        set_Output(LED_ROT_NOTAUS_ANLAGE,    OFF);
+        set_Output(LED_ROT_NOTAUS_SCHRANK,   OFF);
+        set_Output(LED_ROT_NOTAUS_ANLAGE_R,   OFF);
     }
+
 }
 
-int main()
-{
+void send_values(void){
+    char msg[10];
+
+    sprintf(msg, "%d", ADC_reading);
+    mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/state/temp", msg, strlen(msg));
+
+}
+
+void modbus_set_Output(uint8_t slave_adr, uint8_t nr, uint8_t state) {
+  switch (state) {
+    case BLINK:
+        //ioHelperSetBit(outStatesBlinking,  nr,  ON);
+        writeCoil(slave_adr,100+nr,1);
+        wait_write(10);
+        break;
+    //case TOGGLE:
+    //  ioHelperSetBit(outStatesBlinking,  nr,  OFF);
+    //  if (ioHelperReadBit(outStates, nr)) {
+    //    ioHelperSetBit(outStates,  nr,  OFF);
+    //  } else {
+    //    ioHelperSetBit(outStates,  nr,  ON);
+    //  }
+    //  break;
+    case ON:
+      //ioHelperSetBit(outStates,  nr,  ON);
+      //ioHelperSetBit(outStatesBlinking,  nr,  OFF);
+        writeCoil(slave_adr,nr,1);
+        wait_write(10);
+      break;
+    case OFF:
+        writeCoil(slave_adr,nr,0);
+        wait_write(10);
+      //ioHelperSetBit(outStates,  nr,  OFF);
+      //ioHelperSetBit(outStatesBlinking,  nr,  OFF);
+      break;
+  }
+}
+
+uint8_t modbus_read_Input(uint8_t nr, uint8_t type) {
+  uint8_t state = 0;
+  switch (type) {
+    case LEVEL:
+      state = ioHelperReadBit(inStates, nr);
+      break;
+    case EDGE:
+      state = ioHelperReadBit(inStatesBothEdges, nr);
+      ioHelperSetBit(inStatesBothEdges,  nr,  0);
+      break;
+    case RISING:
+      state = ioHelperReadBit(inStatesRisingEdge, nr);
+      ioHelperSetBit(inStatesRisingEdge,  nr,  0);
+      break;
+    case FALLING:
+      state = ioHelperReadBit(inStatesFallingEdge, nr);
+      ioHelperSetBit(inStatesFallingEdge,  nr,  0);
+      break;
+  }
+
+  //reset edges after being read once
+  // if (state) {
+  //   if (type == EDGE)
+  //     ioHelperSetBit(inStatesBothEdges,  nr,  OFF);
+  //   if (type == RISING)
+  //     ioHelperSetBit(inStatesRisingEdge,  nr,  OFF);
+  //   if (type == FALLING)
+  //     ioHelperSetBit(inStatesFallingEdge,  nr,  OFF);
+  // }
+
+  return state;
+}
+
+
+
+int main(){
     // INIT MCU
     avr_init();
     spi_init(); //SPI Master, MODE0, 4Mhz(DIV4), CS_PB.3=HIGH - suitable for WIZNET 5x00(1/2/5)
@@ -136,7 +247,7 @@ int main()
     }
 
     // Subscribe to all topics
-    char SubString[] = "/Filamentanlage/02_Wasserbecken/#";
+    char SubString[] = "/Filamentanlage/01_Extruder/set/#";
     //char SubString[] = "/Filamentanlage/03_Wasserbecken/#";
     mqtt_rc = MQTTSubscribe(&mqtt_client, SubString, QOS0, messageArrived);
     printf("Subscribed (%s) %ld\r\n", SubString, mqtt_rc);
@@ -144,12 +255,15 @@ int main()
 
 
     ioHelperSetBit(outStatesBlinking,  LED_PLC_OK,  1);
+    ioHelperSetBit(outStates,  BitPG5,  1);
 
     uint32_t timer_blink_outs = millis();
     uint32_t timer_send_uptime = millis();
+    uint32_t timer_send_info = millis();
 
-    printf("anlage: %x\n\r", read_Input(IN_ANLAGE_EIN_INV, LEVEL));
+    //printf("anlage: %x\n\r", read_Input(IN_ANLAGE_EIN_INV, LEVEL));
 
+    uint16_t tgl = 0;
     while(1)
     {
         wdt_reset(); // WDT reset at least every sec
@@ -157,12 +271,31 @@ int main()
         ioHelperReadPins();
         ioHelperDebounce();
         ioHelperEdgeDetector();
-        
+
         // Toggle all outs which are set to blinking
         if(millis() - timer_blink_outs > 500){
             outStates[0] ^= outStatesBlinking[0];
             outStates[1] ^= outStatesBlinking[1];
+            outStates[2] ^= outStatesBlinking[2];
+            outStates[3] ^= outStatesBlinking[3];
             timer_blink_outs = millis();
+
+            do_kraftsensor();
+
+            char msg[64];
+            sprintf(msg, "%d", kraftsensor_value);
+            mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/kraft", msg, strlen(msg));
+
+            //tgl ^= 0x01;
+            //modbus_set_Output(4, 0, OFF);
+            //modbus_set_Output(4, 1, ON);
+            //modbus_set_Output(4, 2, BLINK);
+            //modbus_set_Output(4, 3, tgl);
+
+            //readInputReg(4, 0, 1);
+            //uint16_t inp;
+            //wait_receive(1, &inp, 100);
+            //printf("read inputs: 0x%X\n", inp);
         }
 
 #if PLC_MQTT_ENABLED
@@ -171,14 +304,22 @@ int main()
             timer_send_uptime += 5000;
             char msg[64];
             sprintf(msg, "%ld", millis()/1000);
-            mqtt_pub(&mqtt_client, "/Filamentanlage/02_Wasserbecken/uptime", msg, strlen(msg));
+            mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/uptime", msg, strlen(msg));
         }
 #endif
 
-
-        if(read_Input(IN_ANLAGE_EIN_INV, FALLING)){
-            printf("anlage ein\n\r");
+#if PLC_MQTT_ENABLED
+        // send misc info
+        if(millis() - timer_send_info > 200){
+            timer_send_info += 200;
+            send_values(); // 10ms
+            //send_info(); // 27ms every 200ms
         }
+#endif
+
+        //if(read_Input(IN_ANLAGE_EIN_INV, FALLING)){
+        //    printf("anlage ein\n\r");
+        //}
         //    ioHelperSetBit(outStates, LED_GRN_NOTAUS_SCHRANK, 1);
         //}
         //else{
@@ -193,8 +334,8 @@ int main()
         ioHelperSetBit(outStates, LED_BUS_OK, 0);
 #endif
         ioHelperSetOuts();
-    }
-    return 0;
+}
+return 0;
 }
 
 // Timer0
@@ -208,6 +349,40 @@ void timer0_init(void)
     TIMSK0 |= 1<<OCIE0A;	 //IRQ on TIMER0 output compareA
 }
 
+void timer1_init()
+{
+    TCCR3A |= (1<<COM3B1);
+    TCCR3B |= _BV(WGM33);
+
+    ICR3 = 100;
+    OCR3A = 50;
+
+    DDRE |= 1 << 4;
+    TIMSK3 |= 1<<TOIE3;
+
+    TCCR3B |= _BV(CS31);
+}
+
+void timer3_init()
+{
+    TCCR3A |= (1<<COM3B1);
+    TCCR3B |= _BV(WGM33);
+
+    ICR3 = 100;
+    OCR3B = 50;
+
+    DDRE |= 1 << 4;
+    TCCR3B |= _BV(CS31);
+}
+
+void initADC(void)
+{
+    ADMUX = 1 << REFS0 | 0 << REFS1; //Select external Vref
+    ADCSRA = _BV(ADEN) | _BV(ADIE); // enable adc, enable interrupt
+    ADCSRA |= 1 << ADPS2 | 1 << ADPS1 | 1 << ADPS0; // set clock-prescaler to 128
+    ADCSRA |= 1 << ADSC; // start conversion
+}
+
 static void avr_init(void)
 {
     // Initialize device here.
@@ -216,13 +391,21 @@ static void avr_init(void)
     wdt_reset(); // wdt reset ~ every <2000ms
 
     timer0_init();// Timer0 millis engine init
+    //timer3_init();
+    DDRL |= 1<<6;
     uart_init();
 
+    initADC();
+
+    modbus_master_init();
+
     sei(); //re-enable global interrupts
 
     return;
 }
 
+
+
 void print_network_information(void)
 {
 
@@ -245,3 +428,11 @@ void print_network_information(void)
     printf("DNS Server : %d.%d.%d.%d\n\r",gWIZNETINFO.dns[0],gWIZNETINFO.dns[1],gWIZNETINFO.dns[2],gWIZNETINFO.dns[3]);
 }
 
+ISR(ADC_vect)
+{
+    //Reading 10bit conversion result
+    ADC_reading = ADCL;       //copy the first LSB bits
+    ADC_reading |= ADCH << 8; //copy remaing byte
+    ADCSRA |= (1 << ADSC); //Start next conversion
+}
+
diff --git a/modbus-master.c b/modbus-master.c
new file mode 100644
index 0000000..cde854b
--- /dev/null
+++ b/modbus-master.c
@@ -0,0 +1,114 @@
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/delay.h>
+#include "modbus.h"
+
+#define receiveOkay (modbusGetBusState() & (1<<ReceiveCompleted))
+
+void timer2_init()
+{
+    TCCR2A = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
+    TCCR2B|=(1<<CS21); //prescaler 8
+    OCR2A = 200; 
+	TIMSK2|=(1<<OCIE2A);
+}
+
+void modbus_master_init(){
+    modbusSetAddress(1); //better set this to sth.
+	modbusInit();
+    timer2_init(); // modbus tick timer
+}
+
+int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
+
+    uint8_t breaker = timeout;
+    while(!receiveOkay && breaker) { //wait for client response, time out after 1s
+        breaker--;
+        _delay_ms(1);
+        if(breaker==0)
+            return -1;
+    }
+
+    if(receiveOkay) { //if this fails, there was either no response or a crc error
+        if(rxbuffer[1]&0x80) { //client responded with an error code
+            //handle the error
+            return -1;
+        }
+        else {
+            for(uint8_t x=0;x<len;x++) { //rxbuffer[2] should be 8 (4 registers => 8 bytes). You might want to check this at this point.
+                dest[x]=(rxbuffer[3+x*2]<<8)+rxbuffer[4+x*2]; //do sth with the acquired data.
+            }
+        }
+
+    }
+    return 0;
+}
+
+int8_t wait_write(uint8_t timeout){                              
+    uint8_t breaker = timeout;                                   
+    while(!receiveOkay && breaker) { //wait for client response, time out after 1s
+        breaker--;            
+        _delay_ms(1);             
+        if(breaker==0)            
+            return -1;            
+    }                        
+                         
+    if(receiveOkay) { //if this fails, there was either no response or a crc error
+        if(rxbuffer[1]&0x80) { //client responded with an error code
+            return rxbuffer[1]&0x80;                              
+        }                                                         
+    }                                                             
+    return 0;                 
+}
+
+void readReg(uint8_t slaveid, uint16_t address, uint8_t amount) {
+    _delay_ms(2);
+    rxbuffer[0]=slaveid;
+    modbusSetAddress(slaveid);
+    rxbuffer[1]=0x03;
+    rxbuffer[2]=(address>>8)&0xFF;
+    rxbuffer[3]=address&0xFF;
+    rxbuffer[4]=0x00;
+    rxbuffer[5]=amount;
+    modbusSendMessage(5);
+}
+
+void writeReg(uint8_t slaveid, uint16_t address, uint16_t value) {
+    _delay_ms(2);
+    rxbuffer[0]=slaveid;
+    modbusSetAddress(slaveid);
+    rxbuffer[1]=0x06;
+    rxbuffer[2]=(address>>8)&0xFF;
+    rxbuffer[3]=address&0xFF;
+    rxbuffer[4]=(value>>8)&0xFF;;
+    rxbuffer[5]=value&0xFF;
+    modbusSendMessage(5);
+}
+
+void readInputReg(uint8_t slaveid, uint16_t address, uint16_t amount) {
+    _delay_ms(2);
+    rxbuffer[0]=slaveid;
+    modbusSetAddress(slaveid);
+    rxbuffer[1]=0x04;
+    rxbuffer[2]=(address>>8)&0xFF;
+    rxbuffer[3]=address&0xFF;
+    rxbuffer[4]=0;
+    rxbuffer[5]=amount;
+    modbusSendMessage(5);
+}
+
+void writeCoil(uint8_t slaveid, uint16_t address, uint16_t value) {
+    _delay_ms(2);
+    rxbuffer[0]=slaveid;
+    modbusSetAddress(slaveid);
+    rxbuffer[1]=0x05;
+    rxbuffer[2]=(address>>8)&0xFF;
+    rxbuffer[3]=address&0xFF;
+    rxbuffer[4]=value?0xFF:0x00;
+    rxbuffer[5]=0;
+    modbusSendMessage(5);
+}
+
+ISR(TIMER2_COMPA_vect) { //this ISR is called 9765.625 times per second
+	modbusTickTimer();
+}
diff --git a/modbus-master.h b/modbus-master.h
new file mode 100644
index 0000000..0072ec0
--- /dev/null
+++ b/modbus-master.h
@@ -0,0 +1,11 @@
+#include <stdint.h>
+
+void timer2_init(void);
+void modbus_master_init(void);
+int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout);
+int8_t wait_write(uint8_t timeout);
+void readReg(uint8_t slaveid, uint16_t address, uint8_t amount);
+void writeReg(uint8_t slaveid, uint16_t address, uint16_t value);
+void readInputReg(uint8_t slaveid, uint16_t address, uint16_t amount);
+void readInputStatus(uint8_t slaveid, uint16_t address, uint16_t amount);
+void writeCoil(uint8_t slaveid, uint16_t address, uint16_t value);
diff --git a/modbus.c b/modbus.c
new file mode 100644
index 0000000..424e6c3
--- /dev/null
+++ b/modbus.c
@@ -0,0 +1,436 @@
+/*************************************************************************
+Title:    Yet another (small) modbus (server) implementation for the avr.
+Author:   Max Brueggemann
+Hardware: any AVR with hardware UART, tested on Atmega 88/168 at 20Mhz
+License:  BSD-3-Clause
+          
+DESCRIPTION:
+    Refer to the header file yaMBSiavr.h.
+    
+USAGE:
+    Refer to the header file yaMBSiavr.h.
+                    
+LICENSE:
+
+Copyright 2017 Max Brueggemann, www.maxbrueggemann.de
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors
+may be used to endorse or promote products derived from this software without
+specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
+THE POSSIBILITY OF SUCH DAMAGE.
+                        
+*************************************************************************/
+
+#include <avr/io.h>
+#include "modbus.h"
+#include <avr/interrupt.h>
+
+volatile unsigned char BusState = 0;
+volatile uint16_t modbusTimer = 0;
+volatile unsigned char rxbuffer[MaxFrameIndex+1];
+volatile uint16_t DataPos = 0;
+volatile unsigned char PacketTopIndex = 7;
+volatile unsigned char modBusStaMaStates = 0;
+volatile uint16_t modbusDataAmount = 0;
+volatile uint16_t modbusDataLocation = 0;
+
+/* @brief: save address and amount
+*
+*/
+void modbusSaveLocation(void)
+{
+	modbusDataLocation=(rxbuffer[3]|(rxbuffer[2]<<8));
+	if (rxbuffer[1]==fcPresetSingleRegister || rxbuffer[1]==fcForceSingleCoil) modbusDataAmount=1;
+	else modbusDataAmount=(rxbuffer[5]|(rxbuffer[4]<<8));
+}
+
+/* @brief: returns 1 if data location adr is touched by current command
+*
+*         Arguments: - adr: address of the data object
+*
+*/
+uint8_t modbusIsInRange(uint16_t adr)
+{
+        if((modbusDataLocation <= adr) && (adr<(modbusDataLocation+modbusDataAmount)))
+                return 1;
+        return 0;
+}
+
+/* @brief: returns 1 if range of data locations is touched by current command
+*
+*         Arguments: - startAdr: address of first data object in range
+*                    - lastAdr: address of last data object in range
+*
+*/
+uint8_t modbusIsRangeInRange(uint16_t startAdr, uint16_t lastAdr)
+{
+        if(modbusIsInRange(startAdr) && modbusIsInRange(lastAdr))
+                return 1;
+        return 0;
+}
+
+uint8_t modbusGetBusState(void)
+{
+	return BusState;
+}
+
+#if ADDRESS_MODE == SINGLE_ADR
+volatile unsigned char Address = 0x00;
+uint8_t modbusGetAddress(void)
+{
+	return Address;
+}
+
+void modbusSetAddress(unsigned char newadr)
+{
+	Address = newadr;
+}
+#endif
+
+#if PHYSICAL_TYPE == 485
+void transceiver_txen(void)
+{
+	TRANSCEIVER_ENABLE_PORT|=(1<<TRANSCEIVER_ENABLE_PIN);
+	TRANSCEIVER_ENABLE_PORT|=(1<<TRANSCEIVER_ENABLE_PIN_2);
+}
+
+ void transceiver_rxen(void)
+{
+	TRANSCEIVER_ENABLE_PORT&=~(1<<TRANSCEIVER_ENABLE_PIN);
+	TRANSCEIVER_ENABLE_PORT&=~(1<<TRANSCEIVER_ENABLE_PIN_2);
+}
+#endif
+
+/* @brief: A fairly simple Modbus compliant 16 Bit CRC algorithm.
+*
+*  	Returns 1 if the crc check is positive, returns 0 and saves the calculated CRC bytes
+*	at the end of the data array if it fails.
+*  	
+*/
+uint8_t crc16(volatile uint8_t *ptrToArray,uint8_t inputSize) //A standard CRC algorithm
+{
+	uint16_t out=0xffff;
+	uint16_t carry;
+	unsigned char n;
+	inputSize++;
+	for (int l=0; l<inputSize; l++) {
+		out ^= ptrToArray[l];
+		for (n = 0; n < 8; n++) {
+			carry = out & 1;
+			out >>= 1;
+			if (carry) out ^= 0xA001;
+		}
+	}
+	//out=0x1234;
+	if ((ptrToArray[inputSize]==out%256) && (ptrToArray[inputSize+1]==out/256)) //check
+	{
+		return 1;
+	} else { 
+		ptrToArray[inputSize]=out%256; //append Lo
+		ptrToArray[inputSize+1]=out/256; //append Hi
+		return 0;	
+	}
+}
+
+/* @brief: copies a single bit from one char to another char (or arrays thereof)
+*
+*
+*/
+void listBitCopy(volatile uint8_t *source, uint16_t sourceNr,volatile uint8_t *target, uint16_t targetNr)
+{
+	if(*(source+(sourceNr/8))&(1<<(sourceNr-((sourceNr/8)*8))))
+	{
+		*(target+(targetNr/8))|=(1<<(targetNr-((targetNr/8)*8)));
+	} else *(target+(targetNr/8))&=~(1<<(targetNr-((targetNr/8)*8)));
+}
+
+/* @brief: Back to receiving state.
+*
+*/
+void modbusReset(void)
+{
+	BusState=(1<<TimerActive); //stop receiving (error)
+	modbusTimer=0;
+}
+
+void modbusTickTimer(void)
+{
+	if (BusState&(1<<TimerActive)) 
+	{
+		modbusTimer++;
+		if (BusState&(1<<Receiving)) //we are in receiving mode
+		{
+			if ((modbusTimer==modbusInterCharTimeout)) {
+				BusState|=(1<<GapDetected);
+			} else if ((modbusTimer==modbusInterFrameDelayReceiveEnd)) { //end of message
+				#if ADDRESS_MODE == MULTIPLE_ADR
+               		 if (crc16(rxbuffer,DataPos-3)) { //perform crc check only. This is for multiple/all address mode.
+				modbusSaveLocation();
+				BusState=(1<<ReceiveCompleted);
+			 } else modbusReset();
+				#endif
+				#if ADDRESS_MODE == SINGLE_ADR
+				if (rxbuffer[0]==Address && crc16(rxbuffer,DataPos-3)) { //is the message for us? => perform crc check
+					modbusSaveLocation();
+					BusState=(1<<ReceiveCompleted);
+				} else modbusReset();
+				#endif
+			}
+		} else if (modbusTimer==modbusInterFrameDelayReceiveStart){
+            BusState|=(1<<BusTimedOut);
+        }
+	}
+}
+
+ISR(UART_RECEIVE_INTERRUPT)
+{
+    unsigned char data;
+    data = UART_DATA;
+    //printf("%x ", data);
+    modbusTimer=0; //reset timer
+    if (!(BusState & (1<<ReceiveCompleted)) && !(BusState & (1<<TransmitRequested)) && !(BusState & (1<<Transmitting)) && (BusState & (1<<Receiving)) && !(BusState & (1<<BusTimedOut)))
+    {
+        if (DataPos>MaxFrameIndex) modbusReset();
+        else
+        {
+            rxbuffer[DataPos]=data;
+            DataPos++; //TODO: maybe prevent this from exceeding 255?
+        }	    
+    } else 
+        // Bus state is Timed out..
+        if (!(BusState & (1<<ReceiveCompleted)) && !(BusState & (1<<TransmitRequested)) && !(BusState & (1<<Transmitting)) && !(BusState & (1<<Receiving)) && (BusState & (1<<BusTimedOut))) 
+        { 
+            rxbuffer[0]=data;
+            BusState=((1<<Receiving)|(1<<TimerActive));
+            DataPos=1;
+        }
+}
+
+ISR(UART_TRANSMIT_INTERRUPT)
+{
+	BusState&=~(1<<TransmitRequested);
+	BusState|=(1<<Transmitting);
+	UART_DATA=rxbuffer[DataPos];
+	DataPos++;
+	if (DataPos==(PacketTopIndex+1)) {
+		UART_CONTROL&=~(1<<UART_UDRIE);
+	}
+}
+
+ISR(UART_TRANSMIT_COMPLETE_INTERRUPT)
+{
+	#if PHYSICAL_TYPE == 485
+	transceiver_rxen();
+	#endif
+	modbusReset();
+}
+
+void modbusInit(void)
+{
+	UBRRH = (unsigned char)((_UBRR) >> 8);
+	UBRRL = (unsigned char) _UBRR;
+	UART_STATUS = (1<<U2X); //double speed mode.
+#ifdef URSEL   // if UBRRH and UCSRC share the same I/O location , e.g. ATmega8
+	UCSRC = (1<<URSEL)|(3<<UCSZ0); //Frame Size
+#else
+    UCSRC = (3<<UCSZ0); //Frame Size
+#endif
+	UART_CONTROL = (1<<TXCIE)|(1<<RXCIE)|(1<<RXEN)|(1<<TXEN); // USART receiver and transmitter and receive complete interrupt
+	#if PHYSICAL_TYPE == 485
+	TRANSCEIVER_ENABLE_PORT_DDR|=(1<<TRANSCEIVER_ENABLE_PIN);
+	TRANSCEIVER_ENABLE_PORT_DDR|=(1<<TRANSCEIVER_ENABLE_PIN_2);
+	transceiver_rxen();
+	#endif
+	BusState=(1<<TimerActive);
+}
+
+/* @brief: Sends a response.
+*
+*         Arguments: - packtop: Position of the last byte containing data.
+*                               modbusSendException is a good usage example.
+*/
+void modbusSendMessage(unsigned char packtop)
+{
+	PacketTopIndex=packtop+2;
+	crc16(rxbuffer,packtop);
+	BusState|=(1<<TransmitRequested);
+	DataPos=0;
+	#if PHYSICAL_TYPE == 485
+	transceiver_txen();
+	#endif
+	UART_CONTROL|=(1<<UART_UDRIE);
+	BusState&=~(1<<ReceiveCompleted);
+}
+
+/* @brief: Sends an exception response.
+*
+*         Arguments: - exceptionCode
+*                              
+*/
+void modbusSendException(unsigned char exceptionCode)
+{
+	rxbuffer[1]|=(1<<7); //setting MSB of the function code (the exception flag)
+	rxbuffer[2]=exceptionCode; //Exceptioncode. Also the last byte containing data
+	modbusSendMessage(2);
+}
+
+
+/* @brief:  Returns the amount of requested data objects (coils, discretes, registers)
+*
+*/
+uint16_t modbusRequestedAmount(void)
+{
+	return modbusDataAmount;
+}
+
+/* @brief: Returns the address of the first requested data object (coils, discretes, registers)
+*
+*/
+uint16_t modbusRequestedAddress(void)
+{
+	return modbusDataLocation;
+}
+
+/* @brief: copies a single or multiple bytes from one array of bytes to an array of 16-bit-words
+*
+*/
+void intToModbusRegister(volatile uint16_t *inreg, volatile uint8_t *outreg, uint8_t amount)
+{
+	for (uint8_t c=0; c<amount; c++)
+	{
+			*(outreg+c*2) = (uint8_t)(*(inreg+c) >> 8);
+			*(outreg+1+c*2) = (uint8_t)(*(inreg+c));
+	}
+}
+
+/* @brief: copies a single or multiple 16-bit-words from one array of integers to an array of bytes
+*
+*/
+void modbusRegisterToInt(volatile uint8_t *inreg, volatile uint16_t *outreg, uint8_t amount)
+{
+	for (uint8_t c=0; c<amount; c++)
+	{
+		*(outreg+c) = (*(inreg+c*2) << 8) + *(inreg+1+c*2);
+	}
+}
+
+/* @brief: Handles single/multiple register reading and single/multiple register writing.
+*
+*         Arguments: - ptrToInArray: pointer to the user's data array containing registers
+*                    - startAddress: address of the first register in the supplied array
+*                    - size: input array size in the requested format (16bit-registers)
+*
+*/
+uint8_t modbusExchangeRegisters(volatile uint16_t *ptrToInArray, uint16_t startAddress, uint16_t size)
+{
+	if ((modbusDataLocation>=startAddress) && ((startAddress+size)>=(modbusDataAmount+modbusDataLocation))) {
+		
+		if ((rxbuffer[1]==fcReadHoldingRegisters) || (rxbuffer[1]==fcReadInputRegisters) )
+		{
+			if ((modbusDataAmount*2)<=(MaxFrameIndex-4)) //message buffer big enough?
+			{
+				rxbuffer[2]=(unsigned char)(modbusDataAmount*2);
+				intToModbusRegister(ptrToInArray+(modbusDataLocation-startAddress),rxbuffer+3,modbusDataAmount);
+				modbusSendMessage(2+rxbuffer[2]);
+				return 1;
+			} else modbusSendException(ecIllegalDataValue);
+		}
+		else if (rxbuffer[1]==fcPresetMultipleRegisters)
+		{
+			if (((rxbuffer[6])>=modbusDataAmount*2) && ((DataPos-9)>=rxbuffer[6])) //enough data received?
+			{
+				modbusRegisterToInt(rxbuffer+7,ptrToInArray+(modbusDataLocation-startAddress),(unsigned char)(modbusDataAmount));
+				modbusSendMessage(5);
+				return 1;
+			} else modbusSendException(ecIllegalDataValue);//too few data bytes received
+		}
+		else if (rxbuffer[1]==fcPresetSingleRegister)
+		{
+			modbusRegisterToInt(rxbuffer+4,ptrToInArray+(modbusDataLocation-startAddress),1);
+			modbusSendMessage(5);
+			return 1;
+		} 
+		//modbusSendException(ecSlaveDeviceFailure); //inapropriate call of modbusExchangeRegisters
+		return 0;
+		} else {
+		modbusSendException(ecIllegalDataValue);
+		return 0;
+	}
+}
+
+/* @brief: Handles single/multiple input/coil reading and single/multiple coil writing.
+*
+*         Arguments: - ptrToInArray: pointer to the user's data array containing bits
+*                    - startAddress: address of the first bit in the supplied array
+*                    - size: input array size in the requested format (bits)
+*
+*/
+uint8_t modbusExchangeBits(volatile uint8_t *ptrToInArray, uint16_t startAddress, uint16_t size)
+{
+	if ((modbusDataLocation>=startAddress) && ((startAddress+size)>=(modbusDataAmount+modbusDataLocation)))
+	{
+		if ((rxbuffer[1]==fcReadInputStatus) || (rxbuffer[1]==fcReadCoilStatus))
+		{
+			if (modbusDataAmount<=((MaxFrameIndex-4)*8)) //message buffer big enough?
+			{
+				rxbuffer[2]=(modbusDataAmount/8);
+				if (modbusDataAmount%8>0)
+				{
+					rxbuffer[(uint8_t)(modbusDataAmount/8)+3]=0x00; //fill last data byte with zeros
+					rxbuffer[2]++;
+				}
+				for (uint16_t c = 0; c<modbusDataAmount; c++)
+				{
+					listBitCopy(ptrToInArray,modbusDataLocation-startAddress+c,rxbuffer+3,c);
+				}
+				modbusSendMessage(rxbuffer[2]+2);
+				return 1;
+			} else modbusSendException(ecIllegalDataValue); //too many bits requested within single request
+		}
+		else if (rxbuffer[1]==fcForceMultipleCoils)
+		{
+			if (((rxbuffer[6]*8)>=modbusDataAmount) && ((DataPos-9)>=rxbuffer[6])) //enough data received?
+			{
+				for (uint16_t c = 0; c<modbusDataAmount; c++)
+				{
+					listBitCopy(rxbuffer+7,c,ptrToInArray,modbusDataLocation-startAddress+c);
+				}
+				modbusSendMessage(5);
+				return 1;
+			} else modbusSendException(ecIllegalDataValue);//exception too few data bytes received
+		}
+		else if (rxbuffer[1]==fcForceSingleCoil) {
+			listBitCopy(rxbuffer+4,0,ptrToInArray,modbusDataLocation-startAddress);
+			modbusSendMessage(5); 
+			return 1;
+		}
+		//modbusSendException(ecSlaveDeviceFailure); //inanpropriate call of modbusExchangeBits
+		return 0;
+	} else
+	{
+		modbusSendException(ecIllegalDataValue);
+		return 0;
+	}
+}
+
diff --git a/modbus.h b/modbus.h
new file mode 100644
index 0000000..eae774c
--- /dev/null
+++ b/modbus.h
@@ -0,0 +1,421 @@
+#ifndef yaMBIavr_H
+#define yaMBIavr_H
+/************************************************************************
+Title:    Yet another (small) Modbus (server) implementation for the avr.
+Author:   Max Brueggemann
+Hardware: any AVR with hardware UART, tested on Atmega 88/168 at 20Mhz
+License:  BSD-3-Clause
+
+LICENSE:
+
+Copyright 2017 Max Brueggemann, www.maxbrueggemann.de
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors
+may be used to endorse or promote products derived from this software without
+specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
+THE POSSIBILITY OF SUCH DAMAGE.
+    
+************************************************************************/
+#include <avr/io.h>
+/** 
+ *  @code #include <yaMBSIavr.h> @endcode
+ * 
+ *  @brief   Interrupt-based Modbus implementation for small avr microcontrollers.
+ *           The Modbus implementation guidelines at modbus.org call for response
+ *           timeouts in the range of several seconds , hence only timing critical
+ *           parts have been implemented within ISRs. The actual handling of the Modbus
+ *           frame can easily be done in the main while loop.
+ *
+ *  @author Max Brueggemann www.maxbrueggemann.de
+ */
+
+/* define baudrate of modbus */
+#ifndef BAUD
+#define BAUD 38400L
+#endif
+
+/*
+* Definitions for transceiver enable pin.
+*/
+#ifndef TRANSCEIVER_ENABLE_PORT
+#define TRANSCEIVER_ENABLE_PORT PORTJ
+#endif
+
+#ifndef TRANSCEIVER_ENABLE_PIN
+#define TRANSCEIVER_ENABLE_PIN 6
+#define TRANSCEIVER_ENABLE_PIN_2 5
+#endif
+
+#ifndef TRANSCEIVER_ENABLE_PORT_DDR
+#define TRANSCEIVER_ENABLE_PORT_DDR DDRJ
+#endif
+
+/**
+ * @brief    
+ *           At the moment the user has to set the value for Baudrate and
+ *           speed mode manually. The values depend on the operating frequency 
+ *           of your AVR and can be found in its datasheet.
+ */
+#if defined(__AVR_ATtiny2313__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART_UDRE_vect
+#define UART_STATUS   UCSRA
+#define UART_CONTROL  UCSRB
+#define UART_DATA     UDR
+#define UART_UDRIE    UDRIE
+
+#elif defined(__AVR_ATmega164P__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART1_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART1_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART1_UDRE_vect
+#define UART_STATUS   UCSR1A
+#define UART_CONTROL  UCSR1B
+#define UART_DATA     UDR1
+#define UART_UDRIE    UDRIE1
+#define UCSRC UCSR1C
+#define RXCIE RXCIE1
+#define TXCIE TXCIE1
+#define RXEN RXEN1
+#define TXEN TXEN1
+#define UCSZ0 UCSZ10
+#define U2X U2X1
+#define UBRRH UBRR1H
+#define UBRRL UBRR1L
+
+#elif defined(__AVR_ATmega168PA__)|(__AVR_ATmega88PA__)|(__AVR_ATmega328P__)|(__AVR_ATmega168P__)|(__AVR_ATmega88P__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART_UDRE_vect
+#define UART_STATUS   UCSR0A
+#define UART_CONTROL  UCSR0B
+#define UART_DATA     UDR0
+#define UART_UDRIE    UDRIE0
+#define UCSRC UCSR0C
+#define RXCIE RXCIE0
+#define TXCIE TXCIE0
+#define RXEN RXEN0
+#define TXEN TXEN0
+#define UCSZ0 UCSZ00
+#define U2X U2X0
+#define UBRRH UBRR0H
+#define UBRRL UBRR0L
+
+#elif defined(__AVR_ATmega328PB__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART0_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART0_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART0_UDRE_vect
+#define UART_STATUS   UCSR0A
+#define UART_CONTROL  UCSR0B
+#define UART_DATA     UDR0
+#define UART_UDRIE    UDRIE0
+#define UCSRC UCSR0C
+#define RXCIE RXCIE0
+#define TXCIE TXCIE0
+#define RXEN RXEN0
+#define TXEN TXEN0
+#define UCSZ0 UCSZ00
+#define U2X U2X0
+#define UBRRH UBRR0H
+#define UBRRL UBRR0L
+
+#elif defined(__AVR_ATtiny441__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART0_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART0_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART0_UDRE_vect
+#define UART_STATUS   UCSR0A
+#define UART_CONTROL  UCSR0B
+#define UART_DATA     UDR0
+#define UART_UDRIE    UDRIE0
+#define UCSRC UCSR0C
+#define RXCIE RXCIE0
+#define TXCIE TXCIE0
+#define RXEN RXEN0
+#define TXEN TXEN0
+#define UCSZ0 UCSZ00
+#define U2X U2X0
+#define UBRRH UBRR0H
+#define UBRRL UBRR0L
+
+#elif defined(__AVR_ATmega8__)|| defined(__AVR_ATmega16__) || defined(__AVR_ATmega32__) || defined(__AVR_ATmega323__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TXC_vect
+#define UART_RECEIVE_INTERRUPT   USART_RXC_vect
+#define UART_TRANSMIT_INTERRUPT  USART_UDRE_vect
+#define UART_STATUS   UCSRA
+#define UART_CONTROL  UCSRB
+#define UART_DATA     UDR
+#define UART_UDRIE    UDRIE
+
+#elif defined(__AVR_AT90PWM3B__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART_UDRE_vect
+#define UART_STATUS   UCSRA
+#define UART_CONTROL  UCSRB
+#define UART_DATA     UDR
+#define UART_UDRIE    UDRIE
+
+#elif defined(__AVR_ATmega1284P__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART0_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART0_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART0_UDRE_vect
+#define UART_STATUS   UCSR0A
+#define UART_CONTROL  UCSR0B
+#define UART_DATA     UDR0
+#define UART_UDRIE    UDRIE0
+#define UCSRC UCSR0C
+#define RXCIE RXCIE0
+#define TXCIE TXCIE0
+#define RXEN RXEN0
+#define TXEN TXEN0
+#define UCSZ0 UCSZ00
+#define U2X U2X0
+#define UBRRH UBRR0H
+#define UBRRL UBRR0L
+
+#elif defined(__AVR_ATmega2560__)
+#define UART_TRANSMIT_COMPLETE_INTERRUPT USART3_TX_vect
+#define UART_RECEIVE_INTERRUPT   USART3_RX_vect
+#define UART_TRANSMIT_INTERRUPT  USART3_UDRE_vect
+#define UART_STATUS   UCSR3A
+#define UART_CONTROL  UCSR3B
+#define UART_DATA     UDR3
+#define UART_UDRIE    UDRIE3
+#define UCSRC UCSR3C
+#define RXCIE RXCIE3
+#define TXCIE TXCIE3
+#define RXEN RXEN3
+#define TXEN TXEN3
+#define UCSZ0 UCSZ30
+#define U2X U2X0
+#define UBRRH UBRR3H
+#define UBRRL UBRR3L
+
+#else
+#error "no definition available"
+#endif
+
+#ifndef F_CPU
+#error " F_CPU not defined "
+#else
+   #define _UBRR (F_CPU / 8 / BAUD ) -1
+#endif /* F_CPU */
+/*
+ * Available address modes.
+*/
+#define MULTIPLE_ADR 2
+#define SINGLE_ADR 1
+
+/*
+* Use SINGLE_ADR or MULTIPLE_ADR, default: SINGLE_ADR
+* This is useful for building gateways, routers or clients that for whatever reason need multiple addresses.
+*/
+#define ADDRESS_MODE SINGLE_ADR
+
+/*
+* Use 485 or 232, default: 485
+* Use 232 for testing purposes or very simple applications that do not require RS485 and bus topology.
+*/
+#define PHYSICAL_TYPE 485 //possible values: 485, 232
+
+#if BAUD>=19200
+#define modbusInterFrameDelayReceiveStart 16
+#define modbusInterFrameDelayReceiveEnd 18
+#define modbusInterCharTimeout 7
+#else
+#define modbusBlocksize 10
+#define modbusBlockTime ((float)modbusBlocksize*1000000)/((float) BAUD) //is 260 for 38400
+#define timerISROccurenceTime 100 //time in microseconds between two calls of modbusTickTimer
+#define modbusInterFrameDelayReceiveStart  (uint16_t)(modbusBlockTime*3.5/(float)timerISROccurenceTime)
+#define modbusInterFrameDelayReceiveEnd (uint16_t)(modbusBlockTime*4/(float)timerISROccurenceTime)
+#define modbusInterCharTimeout  (uint16_t)(modbusBlockTime*1.5/(float)timerISROccurenceTime)
+#endif
+
+
+/**
+ * @brief    Defines the maximum Modbus frame size accepted by the device. 255 is the default
+ *           and also the maximum value. However, it might be useful to set this to lower
+ *           values, with 8 being the lowest possible value, in order to save on ram space.
+ */
+#define MaxFrameIndex 255
+
+/**
+ * @brief    Modbus Function Codes
+ *           Refer to modbus.org for further information.
+ *           It's good practice to return exception code 01 in case you receive a function code
+ *           that you haven't implemented in your application.
+ */
+#define fcReadCoilStatus 1 //read single/multiple coils
+#define fcReadInputStatus 2 //read single/multiple inputs
+#define fcReadHoldingRegisters 3 //read analog output registers
+#define fcReadInputRegisters 4 //read analog input registers (2 Bytes per register)
+#define fcForceSingleCoil 5 //write single bit
+#define fcPresetSingleRegister 6 //write analog output register (2 Bytes)
+#define fcForceMultipleCoils 15 //write multiple bits
+#define fcPresetMultipleRegisters 16 //write multiple analog output registers (2 Bytes each)
+#define fcReportSlaveID 17 //read device description, run status and other device specific information
+
+/**
+ * @brief    Modbus Exception Codes
+ *           Refer to modbus.org for further information.
+ *           It's good practice to return exception code 01 in case you receive a function code
+ *           that you haven't implemented in your application.
+ */
+#define ecIllegalFunction 1 
+#define ecIllegalDataAddress 2 
+#define ecIllegalDataValue 3
+#define ecSlaveDeviceFailure 4
+#define ecAcknowledge 5
+#define ecSlaveDeviceBusy 6
+#define ecNegativeAcknowledge 7
+#define ecMemoryParityError 8
+
+/**
+ * @brief    Internal bit definitions
+ */
+#define BusTimedOut 0
+#define Receiving 1
+#define Transmitting 2
+#define ReceiveCompleted 3
+#define TransmitRequested 4
+#define TimerActive 5
+#define GapDetected 6
+
+/**
+* @brief    Configures the UART. Call this function only once.
+*/
+extern void modbusInit(void);
+
+/**
+* @brief    receive/transmit data array
+*/
+extern volatile unsigned char rxbuffer[MaxFrameIndex+1];
+
+/**
+* @brief    Current receive/transmit position
+*/
+extern volatile uint16_t DataPos;
+
+/**
+ * This only applies to single address mode.
+ */
+#if ADDRESS_MODE == SINGLE_ADR
+	/**
+	* @brief: Read the device address   
+	*/
+	extern uint8_t modbusGetAddress(void);
+
+	/**
+	* @brief: Set the device address
+	*         Arguments: - newadr: the new device address
+	*/
+	extern void modbusSetAddress(unsigned char newadr);
+#endif
+
+/* @brief: Sends a response.
+*
+*         Arguments: - packtop, index of the last byte in rxbuffer
+*                      that contains payload. Maximum value is
+*				       MaxFrameIndex-2.
+*/
+extern void modbusSendMessage(unsigned char packtop);
+
+/* @brief: Sends a Modbus exception.
+*
+*         Arguments: - exceptionCode
+*/
+extern void modbusSendException(unsigned char exceptionCode);
+
+/* @brief: Discards the current transaction. For MULTIPLE_ADR-mode and general
+*		   testing purposes. Call this function if you don't want to reply at all.
+*/
+void modbusReset(void);
+
+/**
+ * @brief    Call this function whenever possible and check if its return value has the ReceiveCompleted Bit set.
+ *           Preferably do this in the main while. I do not recommend calling this function within ISRs.
+ * @example  if (modbusGetBusState() & (1<<ReceiveCompleted)) {
+ *           modbusSendExcepton(ecIllegalFunction);
+ *           }
+ */
+extern uint8_t modbusGetBusState(void);
+
+/**
+ * @brief    Call every 100us using a timer ISR.
+ */
+extern void modbusTickTimer(void);
+
+/**
+ * @brief    Returns amount of bits/registers requested.
+ */
+extern uint16_t modbusRequestedAmount(void);
+
+/**
+ * @brief    Returns the address of the first requested bit/register.
+ */
+extern uint16_t modbusRequestedAddress(void);
+
+/* A fairly simple and hopefully Modbus compliant 16 Bit CRC algorithm.
+*  Returns 1 if the crc check is positive, returns 0 if it fails.
+*  Appends two crc bytes to the array.
+*/
+extern uint8_t crc16(volatile uint8_t *ptrToArray,uint8_t inputSize);
+
+/* @brief: Handles single/multiple input/coil reading and single/multiple coil writing.
+*
+*         Arguments: - ptrToInArray: pointer to the user's data array containing bits
+*                    - startAddress: address of the first bit in the supplied array
+*                    - size: input array size in the requested format (bits)
+*
+*/
+extern uint8_t modbusExchangeBits(volatile uint8_t *ptrToInArray, uint16_t startAddress, uint16_t size);
+
+/* @brief: Handles single/multiple register reading and single/multiple register writing.
+*
+*         Arguments: - ptrToInArray: pointer to the user's data array containing registers
+*                    - startAddress: address of the first register in the supplied array
+*                    - size: input array size in the requested format (16bit-registers)
+*
+*/
+extern uint8_t modbusExchangeRegisters(volatile uint16_t *ptrToInArray, uint16_t startAddress, uint16_t size);
+
+/* @brief: returns 1 if data location adr is touched by current command
+*
+*         Arguments: - adr: address of the data object
+*
+*/
+extern uint8_t modbusIsInRange(uint16_t adr);
+
+/* @brief: returns 1 if range of data locations is touched by current command
+*
+*         Arguments: - startAdr: address of first data object in range
+*                    - lastAdr: address of last data object in range
+*
+*/
+extern uint8_t modbusIsRangeInRange(uint16_t startAdr, uint16_t lastAdr);
+
+extern volatile uint16_t modbusDataAmount;
+extern volatile uint16_t modbusDataLocation;
+#endif
+
diff --git a/mqtt.c b/mqtt.c
index de27cf5..34eb0e6 100644
--- a/mqtt.c
+++ b/mqtt.c
@@ -4,8 +4,8 @@
 uint8_t mqtt_readBuffer[MQTT_BUFFER_SIZE];
 volatile uint16_t mes_id;
 
-wiz_NetInfo netInfo = { .mac  = {0x00, 0x08, 0xdc, 0xab, 0xcd, 0xf1}, // Mac address
-		.ip   = {192, 168, 2, 1},         // IP address
+wiz_NetInfo netInfo = { .mac  = {0x00, 0x08, 0xdc, 0xab, 0xcd, 0xf2}, // Mac address
+		.ip   = {192, 168, 1, 1},         // IP address
 		.sn   = {255, 255, 0, 0},         // Subnet mask
 		.dns =  {0,0,0,0},			  // DNS address (google dns)
 		.gw   = {192, 168, 0, 1}, // Gateway address
@@ -39,7 +39,7 @@ void mqtt_pub(Client* mqtt_client, char * mqtt_topic, char * mqtt_msg, int mqtt_
 	static uint8_t mqtt_err_cnt = 0;
 	int32_t mqtt_rc;
 
-	printf(">>MQTT pub msg nr%lu ", ++mqtt_pub_count);
+    //printf(">>MQTT pub msg nr%lu ", ++mqtt_pub_count);
 	MQTTMessage pubMessage;
 	pubMessage.qos = QOS0;
 	pubMessage.id = mes_id++;
@@ -50,11 +50,11 @@ void mqtt_pub(Client* mqtt_client, char * mqtt_topic, char * mqtt_msg, int mqtt_
 	if (mqtt_rc == SUCCESSS)
 	{
 		mqtt_err_cnt  = 0;
-		printf(" - OK\r\n");
+		//printf(" - OK\r\n");
 	}
 	else
 	{
-		printf(" - ERROR\r\n");
+		//printf(" - ERROR\r\n");
 		//Reboot device after 20 continuous errors (~ 20sec)
         //while(1);
 		if(mqtt_err_cnt++ > 20)