Initial Battery Monitor Subsystem

src/main/java/frc/robot/constants/BattMonConstants.java

@@ -1,3 +1,56 @@
 package frc.robot.constants;
 
-public class BattMonConstants {}
+public class BattMonConstants {
+  // All of these aren't right and will need to be determined
+
+  // Battery Voltage Conversion
+  public static final double kBattVolt1 = 12;
+  public static final double kBattVolt1DC = 0.9;
+  public static final double kBattVolt2 = 2;
+  public static final double kBattVolt2DC = 0.1;
+  public static final double kBattVoltSlope =
+      (kBattVolt1 - kBattVolt2) / (kBattVolt1DC - kBattVolt2DC);
+  public static final double kBattVoltOffset = kBattVolt1 - kBattVoltSlope * kBattVolt1DC;
+
+  // Battery Current Conversion
+  public static final double kBattCurrent1 = 50;
+  public static final double kBattCurrent1DC = 0.9;
+  public static final double kBattCurrent2 = 1;
+  public static final double kBattCurrent2DC = 0.1;
+  public static final double kBattCurrentSlope =
+      (kBattCurrent1 - kBattCurrent2) / (kBattCurrent1DC - kBattCurrent2DC);
+  public static final double kBattCurrentOffset =
+      kBattCurrent1 - kBattCurrentSlope * kBattCurrent1DC;
+
+  // PDP Voltage Conversion
+  public static final double kPdpVoltage1 = 42;
+  public static final double kPdpVoltage1DC = 0.9;
+  public static final double kPdpVoltage2 = 2;
+  public static final double kPdpVoltage2DC = 0.1;
+  public static final double kPdpVoltSlope =
+      (kPdpVoltage1 - kPdpVoltage2) / (kPdpVoltage1DC - kPdpVoltage2DC);
+  public static final double kPdpVoltOffset = kPdpVoltage1 - kPdpVoltSlope * kPdpVoltage1DC;
+
+  // Breaker Temp Conversion
+  public static double kBreakerTemp1 = 0.32;
+  public static double kBreakerTemp2 = 0.0047;
+
+  // Low Battery Thresholds
+  public static final double kBatt1Low = 2;
+  public static final double kBatt2Low = 2;
+
+  // Temp Limiting
+  public static final double kCurrent1 = 10;
+  public static final double kCurrent1Temp = 200;
+  public static final double kCurrent2 = 200;
+  public static final double kCurrent2Temp = 160;
+  public static final double kTempLimitSlope =
+      (kCurrent1Temp - kCurrent2Temp) / (kCurrent1 - kCurrent2);
+  public static final double kTempLimitOffset = kCurrent1Temp - kTempLimitSlope * kCurrent1;
+  public static final double kHysteresis = 20;
+  public static final double kWarningOffset = 5;
+
+  public static final double kPDPHigh = 42;
+  public static final double kTempHighSlope = 10;
+  public static final double kTempDangerSlope = 15;
+}

src/main/java/frc/robot/subsystems/battMon/BattMonHardware.java

@@ -1,3 +1,40 @@
 package frc.robot.subsystems.battMon;
 
-public class BattMonHardware {}
+import edu.wpi.first.wpilibj.Counter;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycle;
+import frc.robot.constants.BattMonConstants;
+
+public class BattMonHardware implements BattMonIO {
+
+  private DigitalInput battVoltage = new DigitalInput(0);
+  private DigitalInput battCurrent = new DigitalInput(1);
+  private DigitalInput pdpVoltage = new DigitalInput(3);
+  private DigitalInput temp = new DigitalInput(4);
+  private DutyCycle battVoltageCycle = new DutyCycle(battVoltage);
+  private DutyCycle battCurrentCycle = new DutyCycle(battCurrent);
+  private DutyCycle pdpCycle = new DutyCycle(pdpVoltage);
+  private DutyCycle tempCycle = new DutyCycle(temp);
+
+  private Counter tempCounter = new Counter(temp);
+
+  public BattMonHardware() {
+    tempCounter.setUpSourceEdge(true, false);
+  }
+
+  @Override
+  public void updateInputs(BattMonIOInputs inputs) {
+    inputs.batteryVoltage =
+        battVoltageCycle.getOutput() * BattMonConstants.kBattVoltSlope
+            + BattMonConstants.kBattVoltOffset;
+    inputs.batteryCurrent =
+        battCurrentCycle.getOutput() * BattMonConstants.kBattCurrentSlope
+            + BattMonConstants.kBattCurrentOffset;
+    inputs.pdpVoltage =
+        pdpCycle.getOutput() * BattMonConstants.kPdpVoltSlope + BattMonConstants.kPdpVoltOffset;
+    inputs.breakerTemp =
+        ((tempCycle.getHighTimeNanoseconds() / (tempCounter.getPeriod() / 1000000000))
+                - BattMonConstants.kBreakerTemp1)
+            / BattMonConstants.kBreakerTemp2;
+  }
+}

src/main/java/frc/robot/subsystems/battMon/BattMonIO.java

@@ -1,3 +1,16 @@
 package frc.robot.subsystems.battMon;
 
-public class BattMonIO {}
+import org.littletonrobotics.junction.AutoLog;
+
+public interface BattMonIO {
+
+  @AutoLog
+  public class BattMonIOInputs {
+    public double batteryVoltage;
+    public double batteryCurrent;
+    public double pdpVoltage;
+    public double breakerTemp;
+  }
+
+  public default void updateInputs(BattMonIOInputs inputs) {}
+}

src/main/java/frc/robot/subsystems/battMon/BattMonSubsystem.java

@@ -1,3 +1,77 @@
 package frc.robot.subsystems.battMon;
 
-public class BattMonSubsystem {}
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.BattMonConstants;
+import org.littletonrobotics.junction.Logger;
+
+public class BattMonSubsystem extends SubsystemBase {
+  // Private objects
+  private BattMonIO io;
+  private BattMonIOInputsAutoLogged inputs = new BattMonIOInputsAutoLogged();
+  private battMonState curState;
+  // Alerts
+  private Alert highTempAlert = new Alert("WARNING reaching maximum temp!", AlertType.kWarning);
+  private Alert dangerTempAlert = new Alert("DANGER MAXIMUM TEMP REACHED", AlertType.kError);
+  private Alert safeAlert = new Alert("SAFE Temp low. Limits removed", AlertType.kInfo);
+
+  private double recoveryTempThreshold;
+  private double curDangerThreshold;
+  private double curWarnThreshold;
+
+  @Override
+  public void periodic() {
+    // Refresh data and graph it
+    io.updateInputs(inputs);
+    Logger.processInputs(getName(), inputs);
+
+    curDangerThreshold =
+        BattMonConstants.kTempLimitSlope * inputs.batteryCurrent
+            + BattMonConstants.kTempLimitOffset;
+    curWarnThreshold = curDangerThreshold - BattMonConstants.kWarningOffset;
+
+    // Check for dangerous outputs NOTE all of these values are temporary
+    switch (curState) {
+      case NORMAL:
+        if (inputs.breakerTemp >= curWarnThreshold) {
+          highTempAlert.set(true);
+          safeAlert.set(false);
+          recoveryTempThreshold = curWarnThreshold - BattMonConstants.kHysteresis;
+          curState = battMonState.WARNING;
+        } else {
+          break;
+        }
+
+      case WARNING:
+        if (inputs.breakerTemp > curDangerThreshold) {
+          highTempAlert.set(false);
+          dangerTempAlert.set(true);
+          curState = battMonState.DANGER;
+        } else if (inputs.breakerTemp < recoveryTempThreshold) {
+          highTempAlert.set(false);
+          safeAlert.set(true);
+          curState = battMonState.NORMAL;
+        } else {
+          break;
+        }
+
+      case DANGER:
+        if (inputs.breakerTemp <= recoveryTempThreshold) {
+          dangerTempAlert.set(false);
+          highTempAlert.set(true);
+          curState = battMonState.WARNING;
+        }
+    }
+  }
+
+  public battMonState getState() {
+    return curState;
+  }
+
+  public enum battMonState {
+    NORMAL, // Safe Temp
+    WARNING, // Reaching Dangerous temps
+    DANGER // Dangerous temps
+  }
+}