Brownout2 (#18)

Brownout!

src/main/java/frc/robot/GRTSubsystem.java

@@ -0,0 +1,59 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * A convenience class wrapping SubsystemBase for brownout and other shared subsystem logic.
+ */
+public abstract class GRTSubsystem extends SubsystemBase {
+    protected int currentLimit;
+    protected final double minCurrent;
+
+    /**
+     * Creates a GRTSubsystem from a given initial current limit and minimum current.
+     * @param currentLimit The initial (maximum) current limit.
+     * @param minCurrent The minimum current.
+     */
+    public GRTSubsystem(int currentLimit, double minCurrent) {
+        this.currentLimit = currentLimit;
+        this.minCurrent = minCurrent;
+    }
+
+    /**
+     * Gets the subsystem's minimum current.
+     * @return The minimum current.
+     */
+    public double getMinCurrent() {
+        return minCurrent;
+    }
+
+    /**
+     * Gets the subsystem's current current limit.
+     * @return The current limit.
+     */
+    public int getCurrentLimit() {
+        return currentLimit;
+    }
+
+    /**
+     * Gets the total current being drawn by the subsystem. Implementers should use `PowerController.getCurrentDrawnFromPDH`
+     * with all the subsystem's motor controller ports.
+     * @return The total current being drawn by the subsystem.
+     */
+    abstract public double getTotalCurrentDrawn();
+
+    /**
+     * Sets the subsystem's current limit.
+     * @param limit The current limit (in amps) for the subsystem.
+     */
+    abstract public void setCurrentLimit(double limit);
+
+    /**
+     * Contains logic for cleaning up the subsystem for climb.
+     * TODO: this might not be strictly necessary; perhaps it would be useful to call `climbInit()` on all subsystems before climb
+     * and leave the cleanup logic to be implemented (or left blank) in each subsystem, but it's also probably fine to call
+     * `climbInit()` on only the subsystems that need to clean up (and not include it in this class). I guess it depends on how
+     * many subsystems need to implement climb cleanup logic.
+     */
+    public void climbInit() { }
+}

src/main/java/frc/robot/Robot.java

@@ -44,6 +44,9 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // Calculate current limits for subsystems
+    robotContainer.getPowerController().calculateLimits();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.brownout.PowerController;
 import frc.robot.commands.tank.FollowPathCommand;
 import frc.robot.subsystems.tank.TankSubsystem;
 
@@ -39,6 +40,8 @@ public class RobotContainer {
   // Subsystems
   private final TankSubsystem tankSubsystem;
 
+  private final PowerController powerController;
+
   // Controllers
   private XboxController controlXbox = new XboxController(0);
   private JoystickButton xboxAButton = new JoystickButton(controlXbox, XboxController.Button.kA.value);
@@ -48,7 +51,6 @@ public class RobotContainer {
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
-   * 
    */
   public RobotContainer() {
 
@@ -67,6 +69,8 @@ public RobotContainer() {
     // Instantiate subsystems
     tankSubsystem = new TankSubsystem();
 
+    powerController = new PowerController(tankSubsystem);
+
     // Instantiate commands
     // Drive an S-shaped curve from the origin to 3 meters in front through 2 waypoints
     tankCommand = new FollowPathCommand(
@@ -116,4 +120,12 @@ private void controllerBindings() {
   public Command getAutonomousCommand() {
     return tankCommand;
   }
+
+  /**
+   * Gets the PowerController instance.
+   * @return The PowerController instance.
+   */
+  public PowerController getPowerController() {
+    return powerController;
+  }
 }

src/main/java/frc/robot/brownout/PowerController.java

@@ -0,0 +1,85 @@
+package frc.robot.brownout;
+
+import java.util.HashSet;
+import java.util.Set;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import frc.robot.GRTSubsystem;
+
+/**
+ * A power controller which dynamically sets current limits for subsystems to avoid brownout situations.
+ */
+public class PowerController {
+    private static PowerDistribution PDH = new PowerDistribution();
+
+    private GRTSubsystem[] subsystems;
+
+    // TODO: Calculate total sustainable current
+    private static final double totalSustainableCurrent = 200.0;
+
+    public PowerController(GRTSubsystem... subsystems) {
+        this.subsystems = subsystems;
+    }
+
+    /**
+     * Calculates the current limits for each subsystem based on their usage.
+     * This method ensures that all subsystem limits sum to `totalSustainableCurrent`, with dynamic limit distribution
+     * based on usage ratios and minimum current values.
+     */
+    public void calculateLimits() {
+        double totalDraw = PDH.getTotalCurrent();
+        if (totalDraw == 0) return;
+        calculateLimits(totalSustainableCurrent, totalDraw, Set.of(subsystems));
+    }
+
+    /**
+     * Calculates the current limits for each subsystem based on their usage.
+     * This method ensures that all subsystem limits sum to `totalSustainableCurrent`, with dynamic limit distribution
+     * based on usage ratios and minimum current values.
+     * 
+     * @param totalCurrent The total current limit.
+     * @param totalDraw The total current draw.
+     * @param remaining The remaining (non-scaled) subsystems.
+     */
+    private void calculateLimits(double totalCurrent, double totalDraw, Set<GRTSubsystem> remaining) {
+        // Calculate the "ideal ratio" from the total drawn current
+        double idealRatio = totalCurrent / totalDraw;
+
+        // Check each subsystem to see if they cannot be scaled (if the resultant limit would be below their minimum current).
+        // If this is the case, limit the system by its minimum and adjust the ideal ratio accordingly.
+        for (GRTSubsystem subsystem : remaining) {
+            double drawn = subsystem.getTotalCurrentDrawn();
+            double min = subsystem.getMinCurrent();
+
+            if (drawn * idealRatio < min) {
+                subsystem.setCurrentLimit(min);
+
+                // Adjust the ratio to as if the below-minimum subsystem were excluded entirely from the calculation
+                HashSet<GRTSubsystem> cloned = new HashSet<GRTSubsystem>(remaining);
+                cloned.remove(subsystem);
+                calculateLimits(totalCurrent - min, totalDraw - drawn, cloned);
+                return;
+            }
+        }
+
+        // For all other subsystems, limit each subsystem by scaling their current draw by the ideal ratio.
+        // This acts to distribute unused limit to subsystems which are approaching their limits while maintaining that the
+        // sum of all the current limits adds up to the total sustainable threshold.
+        for (GRTSubsystem subsystem : remaining) {
+            subsystem.setCurrentLimit(subsystem.getTotalCurrentDrawn() * idealRatio);
+        }
+    }
+
+    /**
+     * Gets the total current drawn from the PDH by the specified channels.
+     * @param channels The channels to sum.
+     * @return The total current drawn by the provided channels.
+     */
+    public static double getCurrentDrawnFromPDH(int... channels) {
+        double sum = 0;
+        for (int channel : channels) {
+            sum += PDH.getCurrent(channel);
+        }
+        return sum;
+    }
+}

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

@@ -22,11 +22,13 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc.robot.GRTSubsystem;
+import frc.robot.brownout.PowerController;
 
 import static frc.robot.Constants.TankConstants.*;
 
-public class TankSubsystem extends SubsystemBase {
+public class TankSubsystem extends GRTSubsystem {
   private final CANSparkMax leftMain;
   private final CANSparkMax leftFollow;
 
@@ -44,9 +46,13 @@ public class TankSubsystem extends SubsystemBase {
   private final NetworkTableEntry shuffleboardYEntry;
   private final Field2d shuffleboardField;
 
-  public static final double ENCODER_ROTATIONS_TO_METERS = 5 / 92.08;
+  public static final double ENCODER_ROTATIONS_TO_METERS = 5.0 / 92.08;
 
   public TankSubsystem() {
+    // Initialize the subsystem's minimum current and limit
+    // TODO: determine values
+    super(350, 50.0);
+
     // Init left main and follower motors and encoders
     leftMain = new CANSparkMax(fLeftMotorPort, MotorType.kBrushless);
     leftMain.restoreFactoryDefaults();
@@ -182,8 +188,6 @@ public void periodic() {
     shuffleboardXEntry.setDouble(pose.getX());
     shuffleboardYEntry.setDouble(pose.getY());
     shuffleboardField.setRobotPose(pose);
-
-    System.out.println("Odometry readings: " + poseEstimator.getEstimatedPosition());
   }
 
     /**
@@ -241,4 +245,20 @@ public void resetPosition() {
   private double squareInput(double value) {
     return Math.copySign(value * value, value);
   }
+
+  @Override
+  public void setCurrentLimit(double limit) {
+    this.currentLimit = (int) limit;
+
+    int motorLimit = (int) Math.ceil(currentLimit / 4.0); //number of motors
+    leftMain.setSmartCurrentLimit(motorLimit);
+    leftFollow.setSmartCurrentLimit(motorLimit);
+    rightMain.setSmartCurrentLimit(motorLimit);
+    rightFollow.setSmartCurrentLimit(motorLimit);
+  }
+
+  @Override
+  public double getTotalCurrentDrawn() {
+    return PowerController.getCurrentDrawnFromPDH(fLeftMotorPort, fRightMotorPort, bLeftMotorPort, bRightMotorPort);
+  }
 }