diff --git a/main.c b/main.c
index 602a764..4a33385 100644
--- a/main.c
+++ b/main.c
@@ -2,9 +2,9 @@
 #include <util/delay.h>
 #include <modbus.h>
 #include <avr/interrupt.h>
-#include <util/atomic.h>
 
 #include "buffer.h"
+#include "pid.h"
 
 void modbusGet(void);
 
@@ -14,40 +14,9 @@ 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;
-}
+volatile float setpoint_1 = 130; 
+volatile float setpoint_2 = 150;
+volatile float setpoint_3 = 150;
 
 void initADC(void)
 {
@@ -90,19 +59,12 @@ int main(){
     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);
+    init_pid(&controller, 5, 0.2, 0);
 
 
+    // timer for regler
+    TCCR1B|=(1<<CS12) | (1<<CS10);
+	TIMSK1|=(1<<TOIE1)|(1<<OCIE1A);
 
     while(1){
 	    modbusGet();
@@ -123,35 +85,23 @@ void modbusGet(void) {
 	{
 		switch(rxbuffer[1]) {
             case fcReadHoldingRegisters:
-                modbusExchangeRegisters((uint16_t *)&OCR2B,0,1);
-                break;
             case fcPresetSingleRegister:
 			case fcPresetMultipleRegisters: 
 				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);
-                }
+                if(modbusRequestedAddress()<10)
+                    modbusExchangeRegisters((uint16_t *)&adc_avrg,0,8);
                 else
                     modbusExchangeRegisters((uint16_t *)&output,10,4);
-
-			break;
+                break;
 			
             default:
 				modbusSendException(ecIllegalFunction);
@@ -165,7 +115,7 @@ ISR(TIMER0_OVF_vect) { //this ISR is called 9765.625 times per second
 }
 
 ISR(TIMER1_OVF_vect) {
-    output = pid_step(&controller, 1, 180.0 - adc_avrg[1]);
+    output = pid_step(&controller, 1, setpoint_1 - adc_avrg[1]);
 
     PORTD &= ~(1 << 4);
 
@@ -182,14 +132,10 @@ ISR(TIMER1_OVF_vect) {
     }
     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) {
+    /* turn off outputs */
     PORTB &= ~(0x0E);
 }
 
@@ -201,17 +147,17 @@ ISR(ADC_vect)
     //Reading 10bit conversion result
     uint16_t ADC_reading = ADCL;        //copy the first LSB bits
     ADC_reading |= ADCH << 8;           //copy remaing byte
-    ADC_reading *= 10;
+    ADC_reading *= 0.33;
     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;
+            float tmp = (99*adc_avrg[current_channel]) + get_buffer_mean(&adc_buf[current_channel]);
+            tmp /= 100;
+            adc_avrg[current_channel] = tmp;
         }
         else{
-            adc_avrg[current_channel] = get_buffer_mean(&adc_buf[current_channel])/30;
+            adc_avrg[current_channel] = get_buffer_mean(&adc_buf[current_channel]);
             init[current_channel]=0;
         }
     }
@@ -221,7 +167,5 @@ ISR(ADC_vect)
         current_channel = 0;
     set_ADC_Channel(current_channel);
 
-
     ADCSRA |= (1 << ADSC); //Start next conversion
 }
-
diff --git a/makefile b/makefile
index 947663f..50e2c55 100644
--- a/makefile
+++ b/makefile
@@ -1,5 +1,5 @@
 TARGET  = main
-FILES   = main modbus buffer
+FILES   = main modbus buffer pid
 MCU     = atmega328p
 PROGC	= m328p
 CC      = avr-gcc