Diff: speed_pid.h

Revision:

3:7b195612e26d

Parent:

0:7cff999a7f5c

--- a/speed_pid.h	Tue Jul 28 01:42:16 2020 +0000
+++ b/speed_pid.h	Tue Dec 08 01:27:11 2020 +0000
@@ -2,38 +2,77 @@
 #define _SPEED_PID_H_
 
 double taget_speed[6]={0,};
+double ex_taget_speed[6]={0,};
+double dif_taget_speed[6]={0,};
+double filterd_dif_taget_speed[6]={0,0,0,0,0,0,};
+
 double error_speed[6]={0,};
 double Speed_PID_OUTPUT[7]={0,};
-
-void cal_speed_error()
-{
-    for(int i=0;i<6;i++)
-    error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
-    
-}
+double ex_Speed_PID_OUTPUT[7]={0,};
 
 double result_i[6]={0,};
 double filter_result_i[6]={0,};
 double output_i[6]={0,};
 
-void Speed_I()
+
+void cal_speed_error()
 {
     for(int i=0;i<6;i++)
     {
-        if(filter_dif_encoder_data[i]<taget_speed[i])
-            result_i[i]+=1*Speed_Igain[i];
-        else if(filter_dif_encoder_data[i]>taget_speed[i])
-            result_i[i]-=1*Speed_Igain[i]; 
+        if(i < 3 && taget_speed[i] == 0)
+        {
+            error_speed[i] = 0;
+        }
+        else
+        {
+            error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
+        }
+    }
+    for(int i=0;i<6;i++)
+    {
+        dif_taget_speed[i] = ex_taget_speed[i] - taget_speed[i];
+        ex_taget_speed[i]=taget_speed[i];
+        
+        filterd_dif_taget_speed[i] = filterd_dif_taget_speed[i]*0.99 + dif_taget_speed[i]*0.01;
+    }
+    
+}
+
+
+void Speed_I()
+{
+    for(int i=0;i<6;i++)
+    {        
+        result_i[i]+=error_speed[i]*Speed_Igain[i];
+        
+        
+        if(result_i[i]>500)
+            result_i[i]=500;
+        if(result_i[i]<-500)
+            result_i[i]=-500;
             
-        filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i];
-        output_i[i] = - filter_result_i[i];
+        output_i[i] = result_i[i];
     }
 }
 double output_p[6]={0,};
 void Speed_P()
 {
     for(int i=0;i<6;i++)
+    {
         output_p[i] =  error_speed[i] * Speed_Pgain[i];
+    }
+    
+    for(int i=0;i<3;i++)
+    {
+        int p_range = motor_gain[i]*500;
+        
+        if(output_p[i]>p_range)
+            output_p[i]=p_range;
+        if(output_p[i]<-p_range)
+            output_p[i]=-p_range;
+    }
+
+        
 }
 
 void Speed_PID()