From 701fbfc43eb070184b6a7d5c0a50191bbdfc3889 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sun, 15 Feb 2026 14:46:19 -0500 Subject: [PATCH] Some minor changes, either reducing duplicate code, or generalizing code --- .../robot/constants/DrivetrainConstants.java | 9 ++ .../frc/robot/constants/ModuleConstants.java | 18 ++++ .../java/frc/robot/subsystems/Drivetrain.java | 90 ++++++++++++------- .../frc/robot/subsystems/PhotonVision.java | 1 + .../frc/robot/utilities/SwerveModule.java | 29 +++--- 5 files changed, 105 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index e8b35fd..fdd990e 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -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 kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Matrix 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), diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index c91c941..a0c39c2 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -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); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 7f47368..3766c8c 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -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 NOT 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 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(), diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index d7161c6..2358196 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -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()) diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java index 03ef74b..0ca2929 100644 --- a/src/main/java/frc/robot/utilities/SwerveModule.java +++ b/src/main/java/frc/robot/utilities/SwerveModule.java @@ -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,