Add HallEffectSensor and HallEffectBuffer classes

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/HallEffectBuffer.java b/src/org/usfirst/frc/team3501/robot/subsystems/HallEffectBuffer.java
new file mode 100644 (file)
index 0000000..47f6a44
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/HallEffectBuffer.java
@@ -0,0 +1,27 @@
+package org.usfirst.frc.team3501.robot.subsystems;
+
+public class HallEffectBuffer {
+  int length;
+  int index = 0;
+  double[] bufferVals;
+
+  public HallEffectBuffer(int length) {
+    this.length = length;
+    bufferVals = new double[length];
+  }
+
+  public void add(double val) {
+    bufferVals[index++] = val;
+    if (index >= bufferVals.length) {
+      index = 0;
+    }
+  }
+
+  public double getAverage() {
+    double total = 0;
+    for (int i = 0; i < bufferVals.length; i++) {
+      total += bufferVals[i];
+    }
+    return total / bufferVals.length;
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/HallEffectSensor.java b/src/org/usfirst/frc/team3501/robot/subsystems/HallEffectSensor.java
new file mode 100644 (file)
index 0000000..e7bd0c2
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/HallEffectSensor.java
@@ -0,0 +1,27 @@
+package org.usfirst.frc.team3501.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Counter;
+
+public class HallEffectSensor {
+  Counter counter;
+  HallEffectBuffer speeds;
+
+  public HallEffectSensor(Counter counter, int bufferLength) {
+    this.counter = counter;
+    speeds = new HallEffectBuffer(bufferLength);
+  }
+
+  public double getBufferedSpeed() {
+    speeds.add(this.getRPM());
+    return speeds.getAverage();
+  }
+
+  public double getRPS() {
+    return 1 / counter.getPeriod();
+  }
+
+  public double getRPM() {
+    return this.getRPS() * 60;
+  }
+
+}