d-Anteil eingefuegt

HalloRobot/libProject/src/control.c

@@ -60,6 +60,9 @@ float pi_control(float e)
     return new_out;
 }
 
+
+#define K_FEIN 0.4
+#define K_GROB 40.0
 /*
  * Implements PID-controller.
  * Feeds back correction for system input depending on system error e,
@@ -68,32 +71,51 @@ float pi_control(float e)
  */
 float pid_control(float e)
 {
-    static float K = 0.4f;
+    static float K = 5.0;
     const float T_i = 0.1f;//0.18
     const float T_d = 0.025;
-
-    if(e < 0.5 && e > -0.5){
+    /*
+    if(e < 0.45 && e > -0.45){
         K = 0.4f;
     }
     
-    if(e > 2.0 || e < -2.0){
+    if(e > 4.0 || e < -4.0){
         K = 100.0;
     }
+    */
+    /*
+    static float mode = 0; //0=fein, 1= eskalation mit richtung
+    if(e > 2.5){
+        K = K_GROB;
+        mode = 1;
+    }else if(e < - 2.5){
+        K = K_GROB;
+        mode = -1;
+    }
+
+    if(mode == 1 && e < 0.3){
+        mode = 0;
+        K = K_FEIN;
+    }else if(mode == -1 && e > -0.3){
+        K = K_FEIN;
+        mode = 0;
+    }
+    */
     static float e_old = 0;
 
     sum_error += e/F_CONTROL;
     float derivative = (e - e_old)*F_CONTROL;
-
+    e_old = e;
     
 
     float new_out = K*(e + sum_error/T_i + T_d*derivative);
 
     if(new_out>OUT_MAX){
         new_out = OUT_MAX;
-        sum_error = OUT_MAX/300.0;
+        sum_error -= e/F_CONTROL;
     }else if(new_out<OUT_MIN){
         new_out = OUT_MIN;
-        sum_error = OUT_MIN/300.0;
+        sum_error -= e/F_CONTROL;
     } 
 
     return new_out;

HalloRobot/libProject/src/filter.c

@@ -33,7 +33,7 @@ static float iir_filter(float phi_acc)
  */
 float complementary_filter(float phi_acc, float gyro_rate, uint16_t f_sampling)
 {
-    float k = 0.999;
+    float k = 0.995;
     float phi_acc_fil = iir_filter(phi_acc);
     phi_cor = k*(phi_cor+gyro_rate/f_sampling)+(1.0-k)*phi_acc_fil;
     return phi_cor;