Diff: Headers/WeightedAverage.h

Revision:

2:997f57aee3b7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/WeightedAverage.h	Thu Apr 03 13:15:40 2014 +0000
@@ -0,0 +1,54 @@
+#ifndef WEIGHTED_AVERAGE_H
+#define WEIGHTED_AVERAGE_H
+
+// Updated every 4th sample, ~16ms
+#define WEIGHTED_BUFFER_SIZE 64
+
+class WeightedAverage
+{
+    public: 
+        explicit WeightedAverage()
+        {
+            reset();
+        }
+        
+        void add(float reading)
+        {
+            total -= buff[ptr % BUFFER_SIZE];
+            total += reading;
+            buff[ptr++] = reading;
+        }
+        
+        float average()
+        {
+            float avg = 0;
+            
+            int n = (BUFFER_SIZE > ptr ? BUFFER_SIZE : ptr);
+            
+            for(int i = 0; i < n; i++)
+            {
+                avg += (i * buff[i]);   
+            }
+            
+            return avg / ((n * (n + 1)) / 2);
+        }
+        
+        void reset()
+        {
+            ptr = 0;
+            total = 0;
+            for(int i = 0; i < BUFFER_SIZE; i++)
+                buff[i] = 0.0f;
+        }
+        
+    private:
+        unsigned long ptr;
+        float total;
+        float buff [WEIGHTED_BUFFER_SIZE];
+};
+
+WeightedAverage leftWeightedAvg;
+WeightedAverage rightWeightedAvg;
+WeightedAverage frontWeightedAvg;
+
+#endif