regler mit schwellen

HalloRobot/libProject/src/control.c

@@ -68,22 +68,30 @@ float pi_control(float e)
  */
 float pid_control(float e)
 {
-    float K = 0.8f;
+    float K = 0.4f;
     const float T_i = 0.1f;//0.18
-    const float T_d = 0.05;
-   
-    if(e > 2 || e < -2){
-        K = 5.0;
+    const float T_d = 0.025;
+    float ee = e;
+
+    if(e > 10.0 || e < -10.0){
+        //K = 2.4;
+        ee = e*e*e;
+        /*if(e<0){
+            ee = -ee;
+        }*/
+    }
+    
+    if(e > 1.5 || e < -1.5){
+        K = 100.0;
     }
-
     static float e_old = 0;
-
+    /*
     float ee = e*e;
     if(e<0){
          ee = -ee;
     }
-
-    sum_error += ee/F_CONTROL;
+    */
+    sum_error += e/F_CONTROL;
     float derivative = (e - e_old)*F_CONTROL;