diff --git a/main.c b/main.c
index f24f306..602a764 100644
--- a/main.c
+++ b/main.c
@@ -2,24 +2,83 @@
 #include <util/delay.h>
 #include <modbus.h>
 #include <avr/interrupt.h>
+#include <util/atomic.h>
+
+#include "buffer.h"
 
-volatile uint16_t holdingRegisters[4];
 void modbusGet(void);
 
+volatile buffer_t adc_buf[4];
+volatile float adc_avrg[4];
+
+volatile float output;
+volatile struct pid controller;
+
+volatile float setpoint_1 = 180 * 102 / 34; 
+volatile float setpoint_2 = 150 * 102 / 34;
+volatile float setpoint_3 = 150 * 102 / 34;
+
+struct pid {
+    // Controller gains
+    float kP;
+    float kI;
+    float kD;
+
+    // State variables
+    float lastError;
+    float integral;
+};
+
+float pid_step(volatile struct pid* controller, float dt, float error) {
+    // Calculate p term
+    float p = error * controller->kP;
+
+    // Calculate i term
+    ATOMIC_BLOCK(ATOMIC_FORCEON){
+        controller->integral += error * dt * controller->kI;
+        if(controller->integral > 80)
+            controller->integral = 80;
+        if(controller->integral < -80)
+            controller->integral = -80;
+    }
+
+    // Calculate d term, taking care to not divide by zero
+    float d = dt == 0 ? 0 : ((error - controller->lastError) / dt) * controller->kD;
+    controller->lastError = error;
+
+    return p + controller->integral + d;
+}
+
+void initADC(void)
+{
+    ADMUX = 1 << REFS0 | 0 << REFS1; //Select external Vref
+
+    //ADC Status Register A
+    ADCSRA = 1 << ADEN   //Enable ADC
+             | 1 << ADIE //Enable ISR after conversion complete
+             //|  1<<ADATE //Freerunning-Mode
+             //|  1<<ADLAR //2 results bits are left aligned
+             | 1 << ADPS2 //Set clock-prescaler to 128
+             | 1 << ADPS1 | 1 << ADPS0;
+    ADCSRA |= 1 << ADSC; //Start first Conversion for "warmup"
+}
+
 int main(){
     DDRD |= (1 << 4); // LED
     DDRD |= (1 << 3); // FU PWM
     DDRD |= (1 << 2); // 485 DE
 
+    PORTD |= 1 << 4;
+
+    DDRB |= 0x0F; // out channels
+
 	PORTD|=(1<<0); // RX
 
     // FU PWM on PD3
-    OCR2B = 12;
-    TCCR2A |= (1 << COM2B1);
+    TCCR2A |= (1 << COM2B1) | (1 << COM2B0);
     TCCR2A |= (1 << WGM21) | (1 << WGM20);
     TCCR2B |= (1 << CS21);
 
-	sei();
 	modbusSetAddress(1);
     modbusInit();
 
@@ -27,15 +86,36 @@ int main(){
     TCCR0B|=(1<<CS01); //prescaler 8
 	TIMSK0|=(1<<TOIE0);
 
+    initADC();
+    sei();
+
+    _delay_ms(1000);
+    controller = (struct pid){
+        .kP = 5,
+        .kI = 0.2,
+        .kD = 0,
+        .lastError = 0,
+        .integral = 0
+    };
+    //regler
+    TCCR1B|=(1<<CS12) | (1<<CS10);
+    //TCCR1B|=(1<<WGM12);
+	TIMSK1|=(1<<TOIE1)|(1<<OCIE1A);
+
+
+
     while(1){
-        _delay_ms(10);
-        OCR2B += 1;
 	    modbusGet();
     }
 }
 
-ISR(TIMER0_OVF_vect) { //this ISR is called 9765.625 times per second
-	modbusTickTimer();
+void set_ADC_Channel(uint8_t adr)
+{
+    if (adr < 11)
+    {
+        ADMUX &= (0b11110000); //Clear MUX-Address
+        ADMUX |= adr;          //Set new MUX-Address
+    }
 }
 
 void modbusGet(void) {
@@ -43,18 +123,36 @@ void modbusGet(void) {
 	{
 		switch(rxbuffer[1]) {
             case fcReadHoldingRegisters:
-                modbusExchangeRegisters(holdingRegisters,0,4);
+                modbusExchangeRegisters((uint16_t *)&OCR2B,0,1);
                 break;
             case fcPresetSingleRegister:
-				modbusExchangeRegisters(holdingRegisters,0,4);
 			case fcPresetMultipleRegisters: 
-				modbusExchangeRegisters(holdingRegisters,0,4);
-                if(holdingRegisters[0])
-                    PORTD |= 1 << 4;
-                else
-                    PORTD &= ~(1 << 4);
+				modbusExchangeRegisters((uint16_t *)&OCR2B,0,1);
+                break;
 
+            case fcReadCoilStatus:
+				modbusExchangeBits(&PORTB,0,4);
                 break;
+            case fcForceSingleCoil:
+            case fcForceMultipleCoils:
+				modbusExchangeBits(&PORTB,0,4);
+                break;
+
+            case fcReadInputRegisters:
+                if(modbusRequestedAddress()<10){
+                    //;
+                    //float tmp[4];
+                    //tmp[0]=adc_avrg[1]*34/102;
+                    //tmp[1]=adc_avrg[0]*34/102;
+                    //tmp[2]=adc_avrg[3]*34/102;
+                    //tmp[3]=adc_avrg[2]*34/102;
+                    modbusExchangeRegisters(&adc_avrg,0,8);
+                }
+                else
+                    modbusExchangeRegisters((uint16_t *)&output,10,4);
+
+			break;
+			
             default:
 				modbusSendException(ecIllegalFunction);
                 break;
@@ -62,3 +160,68 @@ void modbusGet(void) {
 	}
 }
 
+ISR(TIMER0_OVF_vect) { //this ISR is called 9765.625 times per second
+	modbusTickTimer();
+}
+
+ISR(TIMER1_OVF_vect) {
+    output = pid_step(&controller, 1, 180.0 - adc_avrg[1]);
+
+    PORTD &= ~(1 << 4);
+
+    if(output > 128)
+        output = 128;
+
+    if(output >= 1){
+        uint32_t tmp = output * 512;
+        if(tmp >= 0x10000)
+            tmp = 0xFFFE;
+
+        OCR1A = tmp;
+        PORTB |= 0x0E;
+    }
+    else
+        OCR1A = 1000;
+ 
+
+    //if(adc_avrg[0] < 99 && !(PORTB & 0x01))
+    //    PORTB |= 1 << 0;
+    //if(adc_avrg[0] > 99 && (PORTB & 0x01))
+    //    PORTB &= ~(1 << 0);
+}
+ISR(TIMER1_COMPA_vect) {
+    PORTB &= ~(0x0E);
+}
+
+ISR(ADC_vect)
+{
+    static uint8_t init[4] = {0,0,0,0};
+    static uint8_t current_channel = 0;
+
+    //Reading 10bit conversion result
+    uint16_t ADC_reading = ADCL;        //copy the first LSB bits
+    ADC_reading |= ADCH << 8;           //copy remaing byte
+    ADC_reading *= 10;
+    insert_to_buffer(ADC_reading, &adc_buf[current_channel]);
+
+    if(adc_buf[current_channel].position == BUFFER_SIZE-1){
+        if(init[current_channel]){
+            float tmp = 99*adc_avrg[current_channel];
+            tmp += get_buffer_mean(&adc_buf[current_channel])/30;
+            adc_avrg[current_channel] = tmp/100;
+        }
+        else{
+            adc_avrg[current_channel] = get_buffer_mean(&adc_buf[current_channel])/30;
+            init[current_channel]=0;
+        }
+    }
+
+    current_channel++;
+    if(current_channel == 4)
+        current_channel = 0;
+    set_ADC_Channel(current_channel);
+
+
+    ADCSRA |= (1 << ADSC); //Start next conversion
+}
+
diff --git a/makefile b/makefile
index de07855..947663f 100644
--- a/makefile
+++ b/makefile
@@ -1,5 +1,5 @@
 TARGET  = main
-FILES   = main modbus
+FILES   = main modbus buffer
 MCU     = atmega328p
 PROGC	= m328p
 CC      = avr-gcc