good

diff --git a/src/main/java/frc/robot/commands/gpm/PowerControl.java b/src/main/java/frc/robot/commands/gpm/PowerControl.java
index 0c0043d3bf9ec4e3679c951a09114c16f4e1d2ab..10e3b59e476cdf8375224c6b235c540b9d627aea 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/PowerControl.java
+++ b/src/main/java/frc/robot/commands/gpm/PowerControl.java
@@ -1,8 +1,73 @@
 package frc.robot.commands.gpm;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.PowerControl.Battery;
+import frc.robot.subsystems.PowerControl.EMABreaker;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
 
 public class PowerControl extends Command {
-    // TODO
-    // should be the thing that adjusts current for each subsystem
+    // my beautiful power control subsystems
+    private EMABreaker breaker;
+    private Battery battery;
+    // the real subsystems
+    private Drivetrain drivetrain;
+    private Shooter shooter;
+    private Turret turret;
+    private Hood hood;
+    private Intake intake;
+    private Spindexer spindexer;
+
+    public SeverityLevel severityLevel;
+
+    public enum SeverityLevel {
+        SEVERITY_LVL_ZERO,
+        SEVERITY_LVL_ONE,
+        SEVERITY_LVL_TWO,
+        SEVERITY_LVL_THREE,
+        SEVERITY_LVL_FOUR,
+        SEVERITY_LVL_FIVE,
+    }
+
+    public PowerControl(
+        EMABreaker breaker, // pc
+        Battery battery, // pc
+        Drivetrain drivetrain, // main draw
+        Shooter shooter, // aiming (vital)
+        Turret turret, // aiming
+        Hood hood, // aiming
+        Intake intake, // bps
+        Spindexer spindexer // bps
+    ) {
+        this.breaker = breaker;
+        this.battery = battery;
+        this.drivetrain = drivetrain;
+        this.shooter = shooter;
+        this.turret = turret;
+        this.hood = hood;
+        this.intake = intake;
+        this.spindexer = spindexer;
+
+        addRequirements(breaker, battery); // not sure if I'll need requirement access for setting new current limits
+    }
+
+    @Override
+    public void initialize() {
+        severityLevel = SeverityLevel.SEVERITY_LVL_ZERO;
+    }
+
+    @Override
+    public void execute() {
+        
+    }
+
+    @Override
+    public void end(boolean interupted) {
+        severityLevel = SeverityLevel.SEVERITY_LVL_ZERO; // in the case of disabling this command we shoud reset its effects
+    }
 }
+

diff --git a/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java b/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java
index 48f76920fd23e32ca2b8e2dfc2644cc34ebb29dd..ca915a3683c3970003f810fff45a73122a708c8c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java
+++ b/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java
@@ -108,11 +108,24 @@ public class EMABreaker extends SubsystemBase {
         return false;
     }
 
-    public double percentageUsage() {
+    // returns an average of the filters
+    public double percentageAverageUsage() {
         double sumAvg = 0;
         for (Current f : filters) {
             sumAvg += f.average / f.threshold; // gets percentage of us
         }
         return sumAvg / filters.size(); // average across filters
     }
+
+    // gives the worst case filter
+    public double[] percentageMaxUsage() {
+        Current worst = filters.get(0); // returns worst (default to tau filter)
+        for (Current f : filters) {
+            if (f.average / f.threshold > worst.average / worst.threshold) {
+                worst = f;
+            }
+        }
+        double[] returnValue = {worst.average/worst.threshold, worst.tau};
+        return returnValue;
+    }
 }