Diff: main.cpp

Revision:

2:a83e46376681

Parent:

1:7f23dfc59683

Child:

3:10f685ebc7ef

--- a/main.cpp	Mon Jan 16 17:20:22 2017 +0000
+++ b/main.cpp	Mon Jan 16 19:02:49 2017 +0000
@@ -78,7 +78,11 @@
     char data[6];
     int16_t x, y, z;
     int16_t buffer;
- 
+    
+    int mean[32];
+    int angle;
+    
+    int iteration = 0;
     while(1) {
         accelero.write(device, &XYZ_Data, 1, true);
         accelero.read(device, data, 6);
@@ -93,10 +97,22 @@
         ((char*) &buffer)[1]= data[4];    //MSB
         z = buffer >> 4;
         
-        int theta = std::acos((double)z * z / (double) (x*x + y*y + z*z)) * radtodeg * 100; 
-
-        printDecimalNumber(theta);
-        wait(0.5);
+        if (z > 1024)
+            z = 1024;
+        if (z < -1024)
+            z = -1024;
+        
+        int theta = std::acos((double) z / (double) 1024) * radtodeg * 100;
+        mean[iteration % 32] = theta >> 5;
+        
+        if (iteration % 16 == 0){
+            angle = 0;
+            for (int i = 0; i < 32; i++){
+                angle += mean[i];
+            }
+            printDecimalNumber(angle);
+        }
+        wait(0.006);
+        iteration++;
     }
-    
 }
\ No newline at end of file