tweaks to constants implementation

Author: mwitcpalek

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

@@ -2,19 +2,55 @@
 
 public class BattMonConstants {
   // All of these aren't right and will need to be determined
-  public static double kBattVolt1 = 42;
-  public static double kBattVolt2;
-  public static double kBattCurrent1 = 42;
-  public static double kBattCurrent2;
-  public static double kPdpVoltage1 = 42;
-  public static double kPdpVoltage2;
+
+  // 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;
 
-  public static double kBatt1Low = 2;
-  public static double kBatt2Low = 2;
-  public static double kPDPHigh = 42;
-  public static double kTempHighSlope = 10;
-  public static double kTempDangerSlope = 15;
-  public static double kHysteresis= 1;
+  // 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

@@ -24,14 +24,17 @@ public BattMonHardware() {
 
   @Override
   public void updateInputs(BattMonIOInputs inputs) {
-    inputs.batteryVoltage = battVoltageCycle.getOutput() 
-    * BattMonConstants.kBattVolt1 * BattMonConstants.kBattVolt2;
-    inputs.batteryCurrent = battCurrentCycle.getOutput() 
-    * BattMonConstants.kBattCurrent1 * BattMonConstants.kBattCurrent2;
-    inputs.pdpVoltage = pdpCycle.getOutput() 
-    * BattMonConstants.kPdpVoltage1 * BattMonConstants.kPdpVoltage2;
+    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;
+                - BattMonConstants.kBreakerTemp1)
+            / BattMonConstants.kBreakerTemp2;
   }
 }

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

@@ -4,43 +4,51 @@
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.BattMonConstants;
-import frc.robot.subsystems.battMon.BattMonIO.BattMonIOInputs;
+import org.littletonrobotics.junction.Logger;
 
 public class BattMonSubsystem extends SubsystemBase {
   // Private objects
   private BattMonIO io;
-  private BattMonIOInputs inputs = new BattMonIOInputs();
+  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 tempThreshold;
+  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 >= (BattMonConstants.kTempHighSlope * inputs.pdpVoltage + 100)) {
+        if (inputs.breakerTemp >= curWarnThreshold) {
           highTempAlert.set(true);
           safeAlert.set(false);
-          tempThreshold = inputs.breakerTemp - BattMonConstants.kHysteresis * inputs.pdpVoltage;
+          recoveryTempThreshold = curWarnThreshold - BattMonConstants.kHysteresis;
           curState = battMonState.WARNING;
         } else {
           break;
         }
 
       case WARNING:
-        if (inputs.breakerTemp > (BattMonConstants.kTempDangerSlope * inputs.pdpVoltage)) {
+        if (inputs.breakerTemp > curDangerThreshold) {
           highTempAlert.set(false);
           dangerTempAlert.set(true);
           curState = battMonState.DANGER;
-        } else if (inputs.breakerTemp < tempThreshold) {
+        } else if (inputs.breakerTemp < recoveryTempThreshold) {
           highTempAlert.set(false);
           safeAlert.set(true);
           curState = battMonState.NORMAL;
@@ -49,9 +57,7 @@ public void periodic() {
         }
 
       case DANGER:
-        if (inputs.breakerTemp >= (BattMonConstants.kTempDangerSlope * inputs.pdpVoltage)) {
-          break;
-        } else {
+        if (inputs.breakerTemp <= recoveryTempThreshold) {
           dangerTempAlert.set(false);
           highTempAlert.set(true);
           curState = battMonState.WARNING;