Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Some minor changes, either reducing duplicate code, or generalizing code

Browse Source
This commit is contained in:
Bradley Bickford
2026-02-15 14:46:19 -05:00
parent 7f291e42a1
commit 701fbfc43e
5 changed files with 105 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@@ -1,7 +1,11 @@
package frc.robot.constants;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
public class DrivetrainConstants {
@@ -39,6 +43,11 @@ public class DrivetrainConstants {
public static final double kXTranslationP = .5;
public static final double kYTranslationP = .5;
// TODO How much do we trust gyro and encoders vs vision estimates.
// NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.9, 0.9, 0.9);
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),

18
src/main/java/frc/robot/constants/ModuleConstants.java
View File

@@ -14,6 +14,24 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import edu.wpi.first.math.util.Units;
public class ModuleConstants {
public enum ModuleName {
kFrontLeft("FrontLeft"),
kFrontRight("FrontRight"),
kRearLeft("RearLeft"),
kRearRight("RearRight");
private String loggableName;
private ModuleName(String loggableName) {
this.loggableName = loggableName;
}
public String getLoggableName() {
return "Drivetrain/Modules/" + loggableName;
}
}
// DRIVING MOTOR CONFIG (Kraken)
public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);

90
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@@ -1,8 +1,10 @@
package frc.robot.subsystems;
import java.util.List;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
@@ -27,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.ModuleConstants.ModuleName;
import frc.robot.utilities.SwerveModule;
import frc.robot.utilities.Utilities;
import frc.robot.utilities.VisualPose;
@@ -45,7 +48,7 @@ public class Drivetrain extends SubsystemBase {
public Drivetrain(Pose2d startupPose) {
frontLeft = new SwerveModule(
"FrontLeft",
ModuleName.kFrontLeft,
DrivetrainConstants.kFrontLeftDrivingCANID,
DrivetrainConstants.kFrontLeftTurningCANID,
DrivetrainConstants.kFrontLeftAnalogInPort,
@@ -53,7 +56,7 @@ public class Drivetrain extends SubsystemBase {
);
frontRight = new SwerveModule(
"FrontRight",
ModuleName.kFrontRight,
DrivetrainConstants.kFrontRightDrivingCANID,
DrivetrainConstants.kFrontRightTurningCANID,
DrivetrainConstants.kFrontRightAnalogInPort,
@@ -61,7 +64,7 @@ public class Drivetrain extends SubsystemBase {
);
rearLeft = new SwerveModule(
"RearLeft",
ModuleName.kRearLeft,
DrivetrainConstants.kRearLeftDrivingCANID,
DrivetrainConstants.kRearLeftTurningCANID,
DrivetrainConstants.kRearLeftAnalogInPort,
@@ -69,7 +72,7 @@ public class Drivetrain extends SubsystemBase {
);
rearRight = new SwerveModule(
"RearRight",
ModuleName.kRearRight,
DrivetrainConstants.kRearRightDrivingCANID,
DrivetrainConstants.kRearRightTurningCANID,
DrivetrainConstants.kRearRightAnalogInPort,
@@ -96,7 +99,9 @@ public class Drivetrain extends SubsystemBase {
rearLeft.getPosition(),
rearRight.getPosition()
},
startupPose != null ? startupPose : new Pose2d()
startupPose != null ? startupPose : new Pose2d(),
DrivetrainConstants.kSensorFusionOdometryStdDevs,
DrivetrainConstants.kVisionOdometryStdDevs
);
if(AutoConstants.kAutoConfigOk) {
@@ -141,32 +146,32 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
}
public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
/**
* Can be used to run an individual module on the drive base a static speed while maintaining a static angle.
*
* Good for diagnosing issues with swerve module configuration. Essentially useless otherwise.
*
* @param name The ModuleName enumeration that indicates which module you want to control
* @param staticSpeed The static speed in Meters Per Second to spin the drive wheel at
* @param staticAngleDegrees The static angle in degrees that you want the wheel to face
* @return A complete Command structure that performs the specified action
*/
public Command runIndividualModule(ModuleName name, double staticSpeed, double staticAngleDegrees) {
SwerveModule module = List.of(
frontLeft,
frontRight,
rearLeft,
rearRight
).stream()
.filter((m) -> m.getModuleName() == name)
.findFirst()
.get();
return run(() -> {
frontLeft.setDesiredState(new SwerveModuleState(
module.setDesiredState(new SwerveModuleState(
staticSpeed,
Rotation2d.fromDegrees(staticAngleDegrees)));
});
}
public Command runFrontRight(double staticSpeed, double staticAngleDegrees) {
return run(() -> {
frontRight.setDesiredState(new SwerveModuleState(
staticSpeed,
Rotation2d.fromDegrees(staticAngleDegrees)));
});
}
public Command runRearLeft(double staticSpeed, double staticAngleDegrees) {
return run(() -> {
rearLeft.setDesiredState(new SwerveModuleState(
staticSpeed,
Rotation2d.fromDegrees(staticAngleDegrees)));
});
}
public Command runRearRight(double staticSpeed, double staticAngleDegrees) {
return run(() -> {
rearRight.setDesiredState(new SwerveModuleState(
staticSpeed,
Rotation2d.fromDegrees(staticAngleDegrees)));
Rotation2d.fromDegrees(staticAngleDegrees)
));
});
}
@@ -235,12 +240,37 @@ public class Drivetrain extends SubsystemBase {
* @return A complete Command structure that performs the specified action
*/
public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
return lockRotationToSuppliedPose(
Utilities::getHubPose,
xSpeed,
ySpeed,
rotate180
);
}
/**
* Locks the robots rotation to face a particular pose on the field
*
* This method is <i>NOT</i> for autonomous, see rotateToPose
*
* This method provides a field oriented mechanism of driving the robot, such that the robot
* is always facing the point on the field that is the Pose2d object being supplied. This
* method assumes that the robots estimated pose is reasonably accurate.
*
* @param poseSupplier A Supplier object, lambda, or method reference which consistently produces a Pose2d object to point towards
* @param xSpeed The X (forward/backward) translational speed of the robot
* @param ySpeed The Y (left/right) translational speed of the robot
* @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
* of the robot faces the supplied pose
* @return A complete Command structure that performs the specified action
*/
public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
return runOnce(yawRotationController::reset).andThen(
drive(
xSpeed,
ySpeed,
() -> {
Pose2d faceTowards = Utilities.getHubPose();
Pose2d faceTowards = poseSupplier.get();
Rotation2d targetRotation = new Rotation2d(
faceTowards.getX() - getPose().getX(),

1
src/main/java/frc/robot/subsystems/PhotonVision.java
View File

@@ -134,6 +134,7 @@ public class PhotonVision extends SubsystemBase {
// This is based on what PhotonVision does for multitag Pose estimation
// See PhotonPoseEstimator.multiTagOnCoprocStrategy
// TODO This doesn't currently account for the offset of the tag relative to say the hub
// unclear if that offset amount will be important or not
return Optional.of(Pose3d.kZero
.plus(bestCameraToTarget.inverse())
.relativeTo(CompetitionConstants.kTagLayout.getOrigin())

29
src/main/java/frc/robot/utilities/SwerveModule.java
View File

@@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.AnalogEncoder;
import frc.robot.constants.ModuleConstants;
import frc.robot.constants.ModuleConstants.ModuleName;
/*
* This thread
@@ -30,6 +31,8 @@ import frc.robot.constants.ModuleConstants;
* the controller closed loop controller.
*/
public class SwerveModule {
private ModuleName moduleName;
private TalonFX drive;
private SparkMax turning;
@@ -41,8 +44,6 @@ public class SwerveModule {
private VelocityVoltage driveVelocityRequest;
private String moduleName;
private SwerveModuleState lastTargetState;
private SwerveModuleState lastTargetStateOptimized;
@@ -59,7 +60,7 @@ public class SwerveModule {
* @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
* @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
*/
public SwerveModule(String moduleName, int drivingCANID, int turningCANID) {
public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID) {
this(moduleName, drivingCANID, turningCANID, -1, -1);
}
@@ -73,7 +74,7 @@ public class SwerveModule {
* @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
* @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module
*/
public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
}
@@ -88,7 +89,7 @@ public class SwerveModule {
* @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module
* @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
*/
public SwerveModule(String moduleName, int drivingCANID, int turningCANID,
public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID,
int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
@@ -130,20 +131,20 @@ public class SwerveModule {
this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
this.moduleName = "Drivetrain/Modules/" + moduleName;
this.moduleName = moduleName;
}
public void periodic() {
if(!isAbsoluteEncoderDisabled) {
Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
}
Logger.recordOutput(moduleName + "/ModuleTargetState", lastTargetState);
Logger.recordOutput(moduleName + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState);
Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState());
Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition());
Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition());
// TODO Re-enable this? Was turned off when there was drivetrain issues
// Now that there aren't, do we try this again?
@@ -155,6 +156,10 @@ public class SwerveModule {
}*/
}
public ModuleName getModuleName() {
return moduleName;
}
public SwerveModuleState getState() {
return new SwerveModuleState(
drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,