Stand Freitag 16:00

HalloRobot/libProject/src/control.c

@@ -63,6 +63,8 @@ float pi_control(float e)
 
 #define K_FEIN 0.4
 #define K_GROB 40.0
+#define I_MAX 1.2
+#define I_MIN -1.2
 /*
  * Implements PID-controller.
  * Feeds back correction for system input depending on system error e,
@@ -71,15 +73,15 @@ float pi_control(float e)
  */
 float pid_control(float e)
 {
-    static float K = 5.0;
+    static float K = 4.0;
     const float T_i = 0.1f;//0.18
-    const float T_d = 0.025;
+    const float T_d = 0.04;
     /*
-    if(e < 0.45 && e > -0.45){
-        K = 0.4f;
+    if(e < 1 && e > -1){
+        K = 5.0;
     }
     
-    if(e > 4.0 || e < -4.0){
+    if(e > 3.0 || e < -3.0){
         K = 100.0;
     }
     */
@@ -106,10 +108,12 @@ float pid_control(float e)
     sum_error += e/F_CONTROL;
     float derivative = (e - e_old)*F_CONTROL;
     e_old = e;
+    float p = e;
+    float c_i = sum_error/T_i;
+    float c_d = T_d*derivative;
 
-
-    float new_out = K*(e + sum_error/T_i + T_d*derivative);
-
+    float new_out = K*(p + c_i + c_d);
+    //ezs_printf("%f,%f,%f,%f\n",p,c_i,c_d,new_out);
     if(new_out>OUT_MAX){
         new_out = OUT_MAX;
         sum_error -= e/F_CONTROL;
@@ -117,6 +121,11 @@ float pid_control(float e)
         new_out = OUT_MIN;
         sum_error -= e/F_CONTROL;
     } 
+    if(sum_error>I_MAX){
+        sum_error = I_MAX;
+    }else if(sum_error<I_MIN){
+        sum_error = I_MIN;
+    }  
    
     return new_out;
 }
@@ -163,11 +172,14 @@ float offset_control(float motor_signal)
     const float K = -0.01f;//0.001
     float max_offset = 10.0;
     float min_offset = -10.0;
-    static int i = 0;
+    static int i = 1;
     //sum_offset += K*motor_signal/F_CONTROL;
+    static float sum = 0;
+    sum += motor_signal;
     if(i==10){
-        sum_offset = K*iir_filter_05Hz_N3(motor_signal) + INITIAL_OFFSET; 
+        sum_offset = K*iir_filter_05Hz_N3(sum/10) + INITIAL_OFFSET; 
         i=0;
+        sum = 0;
     }
 
     float new_offset = sum_offset;

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.995;
+    float k = 0.993;
     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;

HalloRobot/main.c

@@ -182,7 +182,7 @@ void control_thread(cyg_addrword_t arg)
         error = null_offset_pos - phi_cor;
         motor_signal = control_func_ptr(error);
         null_offset_pos = offset_control(motor_signal);
-        ezs_printf("%f,%f,%f,%f\n",null_offset_pos,phi_cor,error,motor_signal);
+        //ezs_printf("%f,%f,%f,%f\n",null_offset_pos,phi_cor,error,motor_signal);
         motor_pwm(motor_signal);
 #ifdef WCET
         uint32_t current_run = ezs_watch_stop(&time_sample);