OKay this should work now!

diff --git a/src/main/java/frc/robot/subsystems/Breaker/Breaker.java b/src/main/java/frc/robot/subsystems/Breaker/Breaker.java
index e6644491e45af0e60e43a1cb83ac7ac14a45611c..cf175abfd8f4a333f114242762b8c3e2bbd9f789 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Breaker/Breaker.java
+++ b/src/main/java/frc/robot/subsystems/Breaker/Breaker.java
@@ -1,54 +1,58 @@
 package frc.robot.subsystems.Breaker;
 
 import java.util.ArrayList;
+import java.util.LinkedHashMap;
 import java.util.List;
+import java.util.Map;
 
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
 
 public class Breaker extends SubsystemBase {
+
+    private static class Current {
+        double alpha; // how much of the error we correct per loop
+        double average = 0;
+        double threshold;
+    }
+
     PowerDistribution pDis = new PowerDistribution();
-    List<Double> currents = new ArrayList<>();
+
+    private List<Current> filters = new ArrayList<>(); // contains currents with their alphas and thresholds
 
     public Breaker() {
-        currents.clear();
-    }
-    
-    @Override
-    public void periodic() {
-        currents.add(getCurrentFromPowerDistribution());
-    }
+        for (Map.Entry<Double, Double> entry : BreakerConstants.THRESHOLDS.entrySet()) {
+            double tau = entry.getKey(); // sec
+            double threshold = entry.getValue(); // A
 
-    public double average(double secondsBackward) {
-        // there is a log for every 20ms and thus 50 indexes for each second
-        int totalIndexes = (int) secondsBackward * 50;
-        List<Double> trimmedCurrents = currents.subList(currents.size()-totalIndexes, currents.size());
-        double sum = 0;
-        for (double index : trimmedCurrents) {
-            sum += index;
-        }
-        return sum/trimmedCurrents.size();
-    }
+            Current w = new Current(); // create a filter for the threshold
+            w.threshold = threshold;
+            w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.0198, 1 - e^(-0.02/2) = 0.00995 
 
-    public boolean thresholdPassed(double threshold, double secondsBackward) {
-        return average(secondsBackward) > threshold;
+            filters.add(w);
+        }
     }
 
-    public boolean checkThresholdsBroken() {
-        if (
-            thresholdPassed(BreakerConstants.HALF_SECOND_THRESHOLD_AMPS, 0.5)
-            || thresholdPassed(BreakerConstants.ONE_SECOND_THRESHOLD_AMPS, 1)
-            || thresholdPassed(BreakerConstants.TWO_SECOND_THRESHOLD_AMPS, 2)
-            || thresholdPassed(BreakerConstants.FOUR_SECOND_THRESHOLD_AMPS, 4)
-            || thresholdPassed(BreakerConstants.EIGHT_SECOND_THRESHOLD_AMPS, 8)
-        ) {
-            return true;
-        } else {
-            return false;
+    @Override
+    public void periodic() {
+        double current = getCurrentFromPowerDistribution();
+        for (Current f : filters) {
+            // new avg = old avg + fractionAlpha * difference
+            f.average += f.alpha * (current - f.average);
         }
     }
 
     public double getCurrentFromPowerDistribution() {
         return pDis.getTotalCurrent(); // not using .getCurrent() and then an arguement for the port you can get just one port
     }
+
+    public boolean isOverCurrent() {
+        for (Current f : filters) {
+            if (f.average > f.threshold) {
+                return true; // uh oh
+            }
+        }
+        return false;
+    }
 }

diff --git a/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java b/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java
index eba9dc6e7c58a659c2336d5e915ee95b8015ca68..bbf244f27271ba42ceba562394888afe92fe1578 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java
+++ b/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java
@@ -1,10 +1,25 @@
 package frc.robot.subsystems.Breaker;
 
+import java.util.HashMap;
+import java.util.LinkedHashMap;
+import java.util.Map;
+
 public class BreakerConstants {
-    public static final double HALF_SECOND_THRESHOLD_AMPS = 67.0;
-    public static final double ONE_SECOND_THRESHOLD_AMPS = 67.0;
-    public static final double TWO_SECOND_THRESHOLD_AMPS = 67.0;
-    public static final double FOUR_SECOND_THRESHOLD_AMPS = 67.0;
-    public static final double EIGHT_SECOND_THRESHOLD_AMPS = 67.0;
-    // TODO: add other time thresholds for the breaker
+    public static final Map<Double, Double> THRESHOLDS = new LinkedHashMap<>();
+    static {
+        THRESHOLDS.put(1.0, 600.0);
+        THRESHOLDS.put(2.0, 470.0);
+        THRESHOLDS.put(3.0, 380.0);
+        THRESHOLDS.put(4.0, 340.0);
+        THRESHOLDS.put(5.0, 280.0);
+        THRESHOLDS.put(7.0, 240.0);
+        THRESHOLDS.put(10.0, 200.0);
+        THRESHOLDS.put(15.0, 175.0);
+        THRESHOLDS.put(20.0, 160.0);
+        THRESHOLDS.put(30.0, 150.0);
+        THRESHOLDS.put(100.0, 130.0);
+        THRESHOLDS.put(180.0, 120.0);
+        THRESHOLDS.put(200.0, 110.0);
+        THRESHOLDS.put(500.0, 105.0);
+    }
 }