diff --git a/HalloRobot/libProject/src/control.c b/HalloRobot/libProject/src/control.c
index df0a234738cd3613b532cd2f2efcd68fc3ab122b..98c96eb47fbf32ea132c4ff8817042903d114290 100644
--- a/HalloRobot/libProject/src/control.c
+++ b/HalloRobot/libProject/src/control.c
@@ -68,17 +68,23 @@ float pi_control(float e)
  */
 float pid_control(float e)
 {
-    const float K = 20.0f;
-    const float T_i = 0.18f;
-    const float T_d = 0.00;
-
+    const float K = 0.8f;
+    const float T_i = 0.5f;//0.18
+    const float T_d = 0.05;
 
     static float e_old = 0;
 
-    sum_error += e/F_CONTROL;
+    float ee = e*e;
+    if(e<0){
+         ee = -ee;
+    }
+
+    sum_error += ee/F_CONTROL;
     float derivative = (e - e_old)*F_CONTROL;
 
-    float new_out = K*(e + sum_error/T_i + T_d*derivative);
+    
+
+    float new_out = K*(ee + sum_error/T_i + T_d*derivative);
 
     if(new_out>OUT_MAX){
         new_out = OUT_MAX;
@@ -91,21 +97,36 @@ float pid_control(float e)
     return new_out;
 }
 
-static float iir_filter(float phi_acc)
+static float in[3] = {0};
+static float out[3] = {0};
+
+static float iir_filter_05Hz_N3(float offset)
+{
+    const float a[3] = {-2.9686,2.9377,-0.9691};
+    const float b[4] = {4.7695e-7,1.4309e-6,1.4309e-6,4.7695e-7};
+
+    float new_out = b[0]*offset + b[1]*in[0] + b[2]*in[1] + b[3]*in[2] - a[0]*out[0] - a[1]*out[1] - a[2]*out[2];
+
+    out[2] = out[1];
+    out[1] = out[0];
+    out[0] = new_out;
+    in[2] = in[1];
+    in[1] = in[0];
+    in[0] = offset;    
+    return new_out;
+}
+
+static float iir_filter_1Hz_N2(float offset)
 {
-    static float in_1 = 0;
-    static float in_2 = 0;
-    static float out_1 = 0;
-    static float out_2 = 0;
     const float a[2] = {-1.9112,0.915};
     const float b[3] = {9.4469e-4,0.0019,9.4469e-4};
 
-    float new_out = b[0]*phi_acc + b[1]*in_1 + b[2]*in_2- a[0]*out_1 - a[1]*out_2;
+    float new_out = b[0]*offset + b[1]*in[0] + b[2]*in[1] - a[0]*out[0] - a[1]*out[1];
 
-    out_2 = out_1;
-    out_1 = new_out;
-    in_2 = in_1;
-    in_1 = phi_acc;    
+    out[1] = out[0];
+    out[0] = new_out;
+    in[1] = in[0];
+    in[0] = offset;       
     return new_out;
 }
 
@@ -115,15 +136,17 @@ static float iir_filter(float phi_acc)
  */
 float offset_control(float motor_signal)
 {
-    const float K = 0.001f;//0.001
-    float max_offset = 6.0;
-    float min_offset = 0.0;
-
-    // sum_offset += K*motor_signal/F_CONTROL;
+    const float K = -0.01f;//0.001
+    float max_offset = 10.0;
+    float min_offset = -10.0;
+    static int i = 0;
+    //sum_offset += K*motor_signal/F_CONTROL;
+    if(i==10){
+        sum_offset = K*iir_filter_05Hz_N3(motor_signal) + INITIAL_OFFSET; 
+        i=0;
+    }
 
-    float new_offset = iir_filter(K*motor_signal);
-    // sum_offset = sum_offset + iir_filter(K*motor_signal); 
-    // float new_offset = sum_offset
+    float new_offset = sum_offset;
     if(new_offset > max_offset){
         new_offset = max_offset;
         sum_offset -= motor_signal/F_CONTROL;
@@ -131,7 +154,7 @@ float offset_control(float motor_signal)
         new_offset = min_offset;
         sum_offset -= motor_signal/F_CONTROL;
     }
-
+    i++;
     return new_offset;
 }
 
@@ -140,5 +163,11 @@ float offset_control(float motor_signal)
  */
 void reset_control(void){
     sum_error = 0;
-    sum_offset = INITIAL_OFFSET; 
+    sum_offset = INITIAL_OFFSET;
+    out[2] = 0;
+    out[1] = 0;
+    out[0] = 0;
+    in[2] = 0;
+    in[1] = 0;
+    in[0] = 0;  
 }
diff --git a/HalloRobot/libProject/src/motor.c b/HalloRobot/libProject/src/motor.c
index 6627f431828166f613f26673a9647ba4344f25dc..7426b265ed0028112b8c01a51eece149387974c3 100644
--- a/HalloRobot/libProject/src/motor.c
+++ b/HalloRobot/libProject/src/motor.c
@@ -13,7 +13,7 @@
 #include "diy_int.h"
 
 #define PWM_PERIOD 2048 //20 kHz
-#define PWM_MIN 66 //percent of PWM_PERIOD
+#define PWM_MIN 65 //percent of PWM_PERIOD
 
 static uint16_t usable_pwm_range = 0;
 static uint16_t min_pwm_value = 0;
diff --git a/HalloRobot/main.c b/HalloRobot/main.c
index 6245c321daa9b66fad65f86fd7cf79d873b433f8..abea84da66136edf05579e7cd7f04b967a57a39a 100644
--- a/HalloRobot/main.c
+++ b/HalloRobot/main.c
@@ -181,8 +181,7 @@ void control_thread(cyg_addrword_t arg)
     while(1){
         error = null_offset_pos - phi_cor;
         motor_signal = control_func_ptr(error);
-        null_offset_pos = offset_control(motor_signal);
-        ezs_printf("%f,%f,%f\n",null_offset_pos,phi_cor,motor_signal);
+        null_of/\\\////zs_printf("%f,%f,%f\n",null_offset_pos,phi_cor,motor_signal);
         motor_pwm(motor_signal);
 #ifdef WCET
         uint32_t current_run = ezs_watch_stop(&time_sample);
@@ -295,6 +294,8 @@ static void button_dsr(cyg_vector_t vector, cyg_ucount32 count, cyg_addrword_t d
     }else if(status == ACTIVE){
          cyg_alarm_disable(sampling_alarm_handle);
          cyg_alarm_disable(control_alarm_handle);
+         cyg_thread_suspend(control_handle);
+         cyg_thread_suspend(sampling_handle);
          diy_gpio_clear_pins(GPIOC, GPIO2 | GPIO3); //motoren aus
          reset_filter();
          reset_control();