diff --git a/src/main/java/frc/robot/constants/BattMonConstants.java b/src/main/java/frc/robot/constants/BattMonConstants.java index c5cc557b..785ec559 100644 --- a/src/main/java/frc/robot/constants/BattMonConstants.java +++ b/src/main/java/frc/robot/constants/BattMonConstants.java @@ -1,3 +1,59 @@ package frc.robot.constants; -public class BattMonConstants {} +public class BattMonConstants { + // All of these aren't right and will need to be determined + public static final int kBattCurrentID = 1; + public static final int kBattVoltID = 1; + public static final int kBattTempID = 1; + + // 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; +} diff --git a/src/main/java/frc/robot/subsystems/battMon/BattMonHardware.java b/src/main/java/frc/robot/subsystems/battMon/BattMonHardware.java index 2be2d036..2ccb008a 100644 --- a/src/main/java/frc/robot/subsystems/battMon/BattMonHardware.java +++ b/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; + } +} diff --git a/src/main/java/frc/robot/subsystems/battMon/BattMonIO.java b/src/main/java/frc/robot/subsystems/battMon/BattMonIO.java index 9259b8f1..43d17aa4 100644 --- a/src/main/java/frc/robot/subsystems/battMon/BattMonIO.java +++ b/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) {} +} diff --git a/src/main/java/frc/robot/subsystems/battMon/BattMonSubsystem.java b/src/main/java/frc/robot/subsystems/battMon/BattMonSubsystem.java index 0fc1e1f7..7170db20 100644 --- a/src/main/java/frc/robot/subsystems/battMon/BattMonSubsystem.java +++ b/src/main/java/frc/robot/subsystems/battMon/BattMonSubsystem.java @@ -1,3 +1,75 @@ 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; + } + 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; + } + 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 + } +}