Diff: encoder.h

Revision:

0:c21936a3520a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,35 @@
+#ifndef _ENCODER_H_
+#define _ENCODER_H_
+
+int encoder_data[6]={0,};
+
+int ex_encoder_data[6]={0,};
+int dif_encoder_data[6]={0,};
+double filter_dif_encoder_data[6]={0,};
+double filter_dif_encoder_co[6]     = {0.01,0.01,0.01,0.01,0.01,0.01};
+
+void filter_encoder_data()
+{
+        for(int i=0; i<6;i++)
+        {
+        dif_encoder_data[i]= encoder_data[i] - ex_encoder_data[i];
+        
+        filter_dif_encoder_data[i] = filter_dif_encoder_data[i]*(1-filter_dif_encoder_co[i]) + (double)dif_encoder_data[i]*filter_dif_encoder_co[i];
+        
+        ex_encoder_data[i] = encoder_data[i];
+        }
+}
+
+void encoder_read()
+{
+        encoder_data[0] = encoder1.read();
+        encoder_data[1] = encoder2.read();
+        encoder_data[2] = encoder3.read();
+        encoder_data[3] = encoder4.read();
+        encoder_data[4] = encoder5.read();
+        encoder_data[5] = encoder6.read();
+        
+        filter_encoder_data();
+}
+
+#endif