Diff: Position.h

Revision:

0:e12eb40b9fef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Position.h	Thu Apr 23 00:38:16 2020 +0000
@@ -0,0 +1,98 @@
+#ifndef _POSITION_H_
+#define _POSITION_H_
+
+double taget_position[6]={0,};
+double oder_position[6]={0,};
+
+double now_degree[6]={0,};
+double error_degree[6]={0,};
+
+double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5};
+
+double boot_cnt=0;
+int boot_cnt_max=15000;
+
+void taget_position_read()
+{
+
+    taget_position[0]=first_degree;
+    taget_position[1]=second_degree;
+    taget_position[2]=third_degree;
+    taget_position[3]=-four_degree;
+    taget_position[4]=five_degree;
+    taget_position[5]=six_degree;
+    
+    
+    for(int i = 0; i < 3; i++)
+    {
+        if(taget_position[i] > 160)
+        taget_position[i] = 160;
+        if(taget_position[i] < 0)
+        taget_position[i] = 0;
+    }
+    
+
+        if(taget_position[3] > 0)
+        taget_position[3] = 0;
+        if(taget_position[3] < -152)
+        taget_position[3] = -152;
+
+    
+    for(int i = 4; i < 6; i++)
+    {
+        if(taget_position[i] > 110)
+        taget_position[i] = 110;
+        if(taget_position[i] < 5)
+        taget_position[i] = 5;
+    }
+
+}
+
+void input_filter()
+{
+    
+    
+for(int i=0;i<6;i++)
+    oder_position[i] = oder_position[i]*(1-Position_input_filter[i]*(boot_cnt/boot_cnt_max)) + taget_position[i]*Position_input_filter[i]*(boot_cnt/boot_cnt_max);
+}
+
+void cal_error()
+{
+    for(int i=0;i<6;i++)
+    error_degree[i] = oder_position[i]-now_degree[i];
+}
+
+void cal_degree()
+{
+  for(int i=0;i<6;i++)
+  now_degree[i] = -(((double)encoder_data[i]/100)/1024)*90 * gear[i];
+}
+
+void Position_P()
+{
+        for(int i=0;i<6;i++)
+            taget_speed[i]=-error_degree[i]*position_Pgain[i];
+
+}
+
+
+
+void Position_PID()
+{
+  boot_cnt++;
+    if(boot_cnt>boot_cnt_max)
+    boot_cnt=boot_cnt_max;
+    
+  taget_position_read();
+
+  input_filter();
+        
+  cal_degree();
+  
+  cal_error();
+  
+  Position_P();
+
+}
+
+#endif
\ No newline at end of file