it works, somewhat, and its beautiful

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9530c11d3f232b45779cd696967dba50fc3bc509..ef1aa53a854b30591c6a05618ab1796b43828113 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -37,6 +37,7 @@ import frc.robot.constants.VisionConstants;
 import frc.robot.controls.BaseDriverConfig;
 import frc.robot.controls.Operator;
 import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.Breaker.EMABreaker;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.LED.LED;
@@ -78,6 +79,8 @@ public class RobotContainer {
   private LinearClimb linearClimb = null;
   private LED led = null;
 
+  private EMABreaker breaker = null;
+
   // TODO: move to correct robot and put the correct port?
   private PS5Controller ps5 = new PS5Controller(0);
 
@@ -125,6 +128,7 @@ public class RobotContainer {
         spindexer = new Spindexer();
         intake = new Intake();
         led = new LED();
+        breaker = new EMABreaker();
 
       case WaffleHouse: // AKA Betabot
         turret = new Turret();

diff --git a/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java b/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java
index bbf244f27271ba42ceba562394888afe92fe1578..33492fa9383d982ed805de4015921647ea427387 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java
+++ b/src/main/java/frc/robot/subsystems/Breaker/BreakerConstants.java
@@ -7,19 +7,18 @@ import java.util.Map;
 public class BreakerConstants {
     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);
+        THRESHOLDS.put(1.0, 6.0 * 120); // breaker default at 120
+        THRESHOLDS.put(4.0, 3.4 * 120);
+        THRESHOLDS.put(10.0, 2.0 * 120);
+        THRESHOLDS.put(20.0, 1.6 * 120);
+        THRESHOLDS.put(30.0, 1.5 * 120);
     }
+
+    // ports
+    public static int[] DRIVETRAIN_PORTS = {8,9,10,11,18,19,0,1}; // bls, bld, fld, fls, frs, frd, brd, brs
+    public static int[] TURRET_PORTS = {2};
+    public static int[] INTAKE_PORTS = {15,14,13}; // right, left, roller
+    public static int[] SHOOTER_PORTS = {3,4}; // left, right
+    public static int[] HOOD_PORTS = {5}; // shooter
+    public static int[] SPINDEXER_PORTS = {12}; // spindexer (unupdated on sheets) 
 }

diff --git a/src/main/java/frc/robot/subsystems/Breaker/EMABreaker.java b/src/main/java/frc/robot/subsystems/Breaker/EMABreaker.java
index 244888c08282baca2a0289abf1e38581056866f6..66930842e766ca1c8ec9432114c98bc9317695a8 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Breaker/EMABreaker.java
+++ b/src/main/java/frc/robot/subsystems/Breaker/EMABreaker.java
@@ -5,6 +5,9 @@ import java.util.LinkedHashMap;
 import java.util.List;
 import java.util.Map;
 
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.hal.PowerDistributionJNI;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
@@ -19,8 +22,12 @@ public class EMABreaker extends SubsystemBase {
 
     PowerDistribution pDis = new PowerDistribution();
 
+    double[] subsystemCurrents;
+
     private List<Current> filters = new ArrayList<>(); // contains currents with their alphas and thresholds
+    private List<Current> subsystems = new ArrayList<>();
 
+    private int count = 0;
     public EMABreaker() {
         for (Map.Entry<Double, Double> entry : BreakerConstants.THRESHOLDS.entrySet()) {
             double tau = entry.getKey(); // sec
@@ -32,21 +39,105 @@ public class EMABreaker extends SubsystemBase {
 
             filters.add(w);
         }
+
+        // subsystems
+        for (int i=0; i<pDis.getNumChannels(); i++) {
+            double tau = 1.0;
+            double threshold = i;
+            Current w = new Current();
+            w.threshold = threshold;
+            w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.
+
+            subsystems.add(w);
+        }
     }
 
     @Override
     public void periodic() {
         double current = getCurrentFromPowerDistribution();
+        // this is total current averages
         for (Current f : filters) {
             // new avg = old avg + fractionAlpha * difference
             f.average += f.alpha * (current - f.average);
+            Logger.recordOutput("Breaker/Filter Port" + f.threshold + "Avg", f.average);
+        }
+
+        // this is getting currents coming out of all the ports from PDH (big thing under robot all the wires come out of)
+        subsystemCurrents = getAllCurrentFromPowerDistribution();
+        for (Current s : subsystems) {
+            s.average += s.alpha * (subsystemCurrents[(int) s.threshold] - s.average);
+            Logger.recordOutput("Breaker/Current Port" + s.threshold + "Avg", s.average);
+        }
+
+        Logger.recordOutput("Breaker/TotalCurrent", current);
+        Logger.recordOutput("Breaker/OverCurrent", isOverCurrent());
+
+        // subsystems
+        Logger.recordOutput("Breaker/DrivetrainDraw", getDrivetrainCurrentDraw(subsystemCurrents));
+        Logger.recordOutput("Breaker/SpindexerDraw", getSpindexerCurrentDraw(subsystemCurrents));
+        Logger.recordOutput("Breaker/ShooterDraw", getShooterCurrentDraw(subsystemCurrents));
+        Logger.recordOutput("Breaker/IntakeDraw", getIntakeCurrentDraw(subsystemCurrents));
+        Logger.recordOutput("Breaker/TurretDraw", getTurretCurrentDraw(subsystemCurrents));
+        Logger.recordOutput("Breaker/HoodDraw", getHoodCurrentDraw(subsystemCurrents));
+    }
+
+    public double getDrivetrainCurrentDraw(double[] subsystemCurrents) {
+        double sum = 0;
+        for (int number : BreakerConstants.DRIVETRAIN_PORTS) {
+            sum += subsystemCurrents[number];
+        }
+        return sum;
+    }
+
+    public double getSpindexerCurrentDraw(double[] subsystemCurrents) {
+        double sum = 0;
+        for (int number : BreakerConstants.SPINDEXER_PORTS) {
+            sum += subsystemCurrents[number];
+        }
+        return sum;
+    }
+
+    public double getShooterCurrentDraw(double[] subsystemCurrents) {
+        double sum = 0;
+        for (int number : BreakerConstants.SHOOTER_PORTS) {
+            sum += subsystemCurrents[number];
         }
+        return sum;
     }
 
+    public double getIntakeCurrentDraw(double[] subsystemCurrents) {
+        double sum = 0;
+        for (int number : BreakerConstants.INTAKE_PORTS) {
+            sum += subsystemCurrents[number];
+        }
+        return sum;
+    }
+
+    public double getTurretCurrentDraw(double[] subsystemCurrents) {
+        double sum = 0;
+        for (int number : BreakerConstants.TURRET_PORTS) {
+            sum += subsystemCurrents[number];
+        }
+        return sum;
+    }
+
+    public double getHoodCurrentDraw(double[] subsystemCurrents) {
+        double sum = 0;
+        for (int number : BreakerConstants.HOOD_PORTS) {
+            sum += subsystemCurrents[number];
+        }
+        return sum;
+    }
+    
+
     public double getCurrentFromPowerDistribution() {
         return pDis.getTotalCurrent(); // not using .getCurrent() and then an arguement for the port you can get just one port
     }
 
+    public double[] getAllCurrentFromPowerDistribution() {
+        return pDis.getAllCurrents();
+    }
+
     public boolean isOverCurrent() {
         for (Current f : filters) {
             if (f.average > f.threshold) {