Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/main/java/org/team5924/frc2025/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -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 # */
Expand Down
13 changes: 13 additions & 0 deletions src/main/java/org/team5924/frc2025/subsystems/drive/Module.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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(
Expand All @@ -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() {
Expand All @@ -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. */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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[] {};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -83,6 +84,7 @@ public class ModuleIOTalonFX implements ModuleIO {
private final StatusSignal<AngularVelocity> driveVelocity;
private final StatusSignal<Voltage> driveAppliedVolts;
private final StatusSignal<Current> driveCurrent;
private final StatusSignal<Temperature> driveTemp;

// Inputs from turn motor
private final StatusSignal<Angle> turnAbsolutePosition;
Expand All @@ -91,6 +93,7 @@ public class ModuleIOTalonFX implements ModuleIO {
private final StatusSignal<AngularVelocity> turnVelocity;
private final StatusSignal<Voltage> turnAppliedVolts;
private final StatusSignal<Current> turnCurrent;
private final StatusSignal<Temperature> turnTemp;

// Connection debouncers
private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
Expand Down Expand Up @@ -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();
Expand All @@ -175,6 +179,7 @@ public ModuleIOTalonFX(
turnVelocity = turnTalon.getVelocity();
turnAppliedVolts = turnTalon.getMotorVoltage();
turnCurrent = turnTalon.getStatorCurrent();
turnTemp = turnTalon.getDeviceTemp();

// Configure periodic frames
BaseStatusSignal.setUpdateFrequencyForAll(
Expand All @@ -187,17 +192,21 @@ public ModuleIOTalonFX(
turnAbsolutePosition,
turnVelocity,
turnAppliedVolts,
turnCurrent);
turnCurrent,
driveTemp,
turnTemp);
ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon);
}

@Override
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
Expand All @@ -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());
Expand All @@ -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 =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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();
}

Expand Down Expand Up @@ -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());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<CoralInAndOut.CoralState> {
Expand Down