Diff: speed_pid.h

Revision:

0:c21936a3520a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_pid.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,43 @@
+double taget_speed[6]={0,};
+double error_speed[6]={0,};
+double Speed_PID_OUTPUT[6]={0,};
+
+void cal_speed_error()
+{
+    for(int i=0;i<6;i++)
+    error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
+}
+
+double result_i[6]={0,};
+double filter_result_i[6]={0,};
+double output_i[6]={0,};
+
+void Speed_I()
+{
+    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]; 
+            
+        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];
+    }
+}
+double output_p[6]={0,};
+void Speed_P()
+{
+    for(int i=0;i<6;i++)
+        output_p[i] = - error_speed[i] * Speed_Pgain[i];
+}
+
+void Speed_PID()
+{
+    cal_speed_error();
+    Speed_I();
+    Speed_P();
+    
+    for(int i=0;i<6;i++)
+    Speed_PID_OUTPUT[i] = output_i[i] + output_p[i];
+}