Diff: speed_pid.h

Revision:

0:e12eb40b9fef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_pid.h	Thu Apr 23 00:38:16 2020 +0000
@@ -0,0 +1,59 @@
+#ifndef _SPEED_PID_H_
+#define _SPEED_PID_H_
+
+double taget_speed[6]={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 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];
+        if(i > 2)
+        {
+            Speed_PID_OUTPUT[i] *= -1;
+        }
+    }
+    
+    //pc.printf("%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f",error_speed[0],filter_dif_encoder_data[0],taget_speed[0],Speed_PID_OUTPUT[0],output_i[0],output_p[0]);
+    //pc.printf("\n");
+    
+}
+
+#endif
\ No newline at end of file