diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 5fe0ab8..4aef63d 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -47,6 +47,7 @@ public static enum Mode { /* General */ public static final double LOOP_PERIODIC_SECONDS = 0.02; + public static final double MOTOR_MAX_TEMP = 95; /* ### Subsystems ### */ /* # Rollers # */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java index 0eaff9c..f5919ea 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.Constants; public class Module { private final ModuleIO io; @@ -38,6 +39,8 @@ public class Module { private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; + private final Alert driveOverTempAlert; + private final Alert turnOverTempAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module( @@ -59,6 +62,14 @@ public Module( new Alert( "Disconnected turn encoder on module " + Integer.toString(index) + ".", AlertType.kError); + driveOverTempAlert = + new Alert( + "Drive motor on module " + Integer.toString(index) + " is overheating.", + AlertType.kWarning); + turnOverTempAlert = + new Alert( + "Turn motor on module " + Integer.toString(index) + " is overheating.", + AlertType.kWarning); } public void periodic() { @@ -78,6 +89,8 @@ public void periodic() { driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + driveOverTempAlert.set(inputs.driveTempCelsius > Constants.MOTOR_MAX_TEMP); + turnOverTempAlert.set(inputs.turnTempCelsius > Constants.MOTOR_MAX_TEMP); } /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java index 726a313..acb5307 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java @@ -27,6 +27,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; + public double driveTempCelsius = 0.0; public boolean turnConnected = false; public boolean turnEncoderConnected = false; @@ -35,6 +36,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; + public double turnTempCelsius = 0.0; public double[] odometryTimestamps = new double[] {}; public double[] odometryDrivePositionsRad = new double[] {}; diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java index e6c8cdb..af0831e 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java @@ -42,6 +42,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import java.util.Queue; import org.team5924.frc2025.generated.TunerConstants; @@ -83,6 +84,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveCurrent; + private final StatusSignal driveTemp; // Inputs from turn motor private final StatusSignal turnAbsolutePosition; @@ -91,6 +93,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; + private final StatusSignal turnTemp; // Connection debouncers private final Debouncer driveConnectedDebounce = new Debouncer(0.5); @@ -167,6 +170,7 @@ public ModuleIOTalonFX( driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); + driveTemp = driveTalon.getDeviceTemp(); // Create turn status signals turnAbsolutePosition = cancoder.getAbsolutePosition(); @@ -175,6 +179,7 @@ public ModuleIOTalonFX( turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); + turnTemp = turnTalon.getDeviceTemp(); // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( @@ -187,7 +192,9 @@ public ModuleIOTalonFX( turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnCurrent, + driveTemp, + turnTemp); ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); } @@ -195,9 +202,11 @@ public ModuleIOTalonFX( public void updateInputs(ModuleIOInputs inputs) { // Refresh all signals var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + BaseStatusSignal.refreshAll( + drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, driveTemp); var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + BaseStatusSignal.refreshAll( + turnPosition, turnVelocity, turnAppliedVolts, turnCurrent, turnTemp); var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); // Update drive inputs @@ -206,6 +215,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + inputs.driveTempCelsius = driveTemp.getValueAsDouble(); // Update turn inputs inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); @@ -215,6 +225,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + inputs.turnTempCelsius = turnTemp.getValueAsDouble(); // Update odometry inputs inputs.odometryTimestamps = diff --git a/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java index 30e593f..561c77c 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java @@ -16,21 +16,21 @@ package org.team5924.frc2025.subsystems.elevator; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2025.Constants; -import org.team5924.frc2025.RobotState; -import org.team5924.frc2025.util.LoggedTunableNumber; - import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lombok.Getter; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.Constants; +import org.team5924.frc2025.RobotState; +import org.team5924.frc2025.util.LoggedTunableNumber; public class Elevator extends SubsystemBase { // Tolerance for position control (in meters) @@ -66,14 +66,22 @@ public enum ElevatorState { private final Alert leftMotorDisconnected; private final Alert rightMotorDisconnected; + private final Alert leftMotorOverTemp; + private final Alert rightMotorOverTemp; + public Elevator(ElevatorIO io) { this.io = io; this.goalState = ElevatorState.MANUAL; RobotState.getInstance().setElevatorState(this.goalState); this.leftMotorDisconnected = - new Alert("Left elevator motor disconnected!", Alert.AlertType.kWarning); + new Alert("Left elevator motor disconnected!", Alert.AlertType.kError); this.rightMotorDisconnected = - new Alert("Right elevator motor disconnected!", Alert.AlertType.kWarning); + new Alert("Right elevator motor disconnected!", Alert.AlertType.kError); + + this.leftMotorOverTemp = + new Alert("Left elevator motor over temperature!", Alert.AlertType.kWarning); + this.rightMotorOverTemp = + new Alert("Right elevator motor over temperature!", Alert.AlertType.kWarning); upSysId = new SysIdRoutine( @@ -107,6 +115,9 @@ public void periodic() { leftMotorDisconnected.set(!inputs.leftMotorConnected); rightMotorDisconnected.set(!inputs.rightMotorConnected); + leftMotorOverTemp.set(inputs.leftTempCelsius > Constants.MOTOR_MAX_TEMP); + rightMotorOverTemp.set(inputs.rightTempCelsius > Constants.MOTOR_MAX_TEMP); + io.periodicUpdates(); } @@ -139,7 +150,9 @@ public void setGoalState(ElevatorState goalState) { this.goalState = goalState; switch (goalState) { case MANUAL -> RobotState.getInstance().setElevatorState(ElevatorState.MANUAL); - case MOVING -> DriverStation.reportError("MOVING is an intermediate state and cannot be set as a goal state!", null); + case MOVING -> + DriverStation.reportError( + "MOVING is an intermediate state and cannot be set as a goal state!", null); default -> { RobotState.getInstance().setElevatorState(ElevatorState.MOVING); io.setHeight(goalState.heightMeters.getAsDouble()); diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java index 36e46b2..1b146a1 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java @@ -17,17 +17,15 @@ package org.team5924.frc2025.subsystems.rollers.CoralInAndOut; import java.util.function.DoubleSupplier; - +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2025.RobotState; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem.VoltageState; import org.team5924.frc2025.util.LoggedTunableNumber; -import lombok.Getter; -import lombok.RequiredArgsConstructor; -import lombok.Setter; - @Setter @Getter public class CoralInAndOut extends GenericRollerSystem {