diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index c41cdd2..5e73a2f 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -15,6 +15,7 @@ import frc.robot.superstructure.Superstructure; import frc.robot.swerve.Swerve; import frc.robot.swerve.SwerveConstants; +import frc.robot.swerve.SwerveFactory; /** Auto subsystem. */ public class Auto extends Subsystem { @@ -49,7 +50,7 @@ private Auto() { new PIDConstants(1.0), new PIDConstants(1.0), SwerveConstants.MAXIMUM_SPEED, - SwerveConstants.NORTH_EAST_MODULE_CONFIG.position().getNorm(), + SwerveFactory.createNorthEastModuleTranslation().getNorm(), // TODO new ReplanningConfig()), AllianceFlipHelper::shouldFlip, swerve); diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 2feb045..1f27cef 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -27,17 +27,17 @@ public class Swerve extends Subsystem { /** Initializes the swerve subsystem and configures swerve hardware. */ private Swerve() { - swerveModules[0] = SwerveFactory.createModule(SwerveConstants.NORTH_WEST_MODULE_CONFIG); - swerveModules[1] = SwerveFactory.createModule(SwerveConstants.NORTH_EAST_MODULE_CONFIG); - swerveModules[2] = SwerveFactory.createModule(SwerveConstants.SOUTH_EAST_MODULE_CONFIG); - swerveModules[3] = SwerveFactory.createModule(SwerveConstants.SOUTH_WEST_MODULE_CONFIG); + swerveModules[0] = SwerveFactory.createNorthWestModule(); + swerveModules[1] = SwerveFactory.createNorthEastModule(); + swerveModules[2] = SwerveFactory.createSouthEastModule(); + swerveModules[3] = SwerveFactory.createSouthWestModule(); swerveKinematics = new SwerveDriveKinematics( - SwerveConstants.NORTH_WEST_MODULE_CONFIG.position(), - SwerveConstants.NORTH_EAST_MODULE_CONFIG.position(), - SwerveConstants.SOUTH_EAST_MODULE_CONFIG.position(), - SwerveConstants.SOUTH_WEST_MODULE_CONFIG.position()); + SwerveFactory.createNorthWestModuleTranslation(), + SwerveFactory.createNorthEastModuleTranslation(), + SwerveFactory.createSouthEastModuleTranslation(), + SwerveFactory.createSouthWestModuleTranslation()); } /** diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 7ab366c..663fed1 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -1,7 +1,6 @@ package frc.robot.swerve; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.lib.config.FeedbackControllerConfig; import frc.lib.config.FeedforwardControllerConfig; @@ -32,37 +31,6 @@ public static class MK4iConstants { /** Module Y offset in meters. */ public static final double Y_OFFSET = Units.inchesToMeters(10.375); - /** Swerve's CAN bus. */ - public static final String SWERVE_BUS = "swerve"; - - /** Module configuration for the north west swerve module. */ - public static final SwerveModuleConfig NORTH_WEST_MODULE_CONFIG = - new SwerveModuleConfig( - new SwerveModuleCAN(16, 8, 24, SWERVE_BUS), - new Translation2d(X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.084717).unaryMinus()); - - /** Module configuration for the north east swerve module. */ - public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG = - new SwerveModuleConfig( - new SwerveModuleCAN(18, 16, 30, SWERVE_BUS), - new Translation2d(X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(0.196777).unaryMinus()); - - /** Module configuration for the south east swerve module. */ - public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = - new SwerveModuleConfig( - new SwerveModuleCAN(22, 12, 26, SWERVE_BUS), - new Translation2d(-X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(0.276611).unaryMinus()); - - /** Module configuration for the south west swerve module. */ - public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG = - new SwerveModuleConfig( - new SwerveModuleCAN(20, 10, 28, SWERVE_BUS), - new Translation2d(-X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.276855).plus(Rotation2d.fromDegrees(180)).unaryMinus()); - /** Maximum speed in meters per second. */ public static final double MAXIMUM_SPEED = 4.5; diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index 5442c07..56a642c 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -1,6 +1,8 @@ package frc.robot.swerve; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import frc.lib.CAN; import frc.lib.config.AbsoluteEncoderConfig; import frc.lib.controller.PositionControllerIO; @@ -13,7 +15,7 @@ import frc.robot.RobotConstants; import frc.robot.RobotConstants.Subsystem; -/** Helper class for creating hardware for the swerve subsystem. */ +/** Factory for creating swerve subsystem hardware. */ public class SwerveFactory { /** @@ -21,8 +23,96 @@ public class SwerveFactory { * * @return a swerve module. */ - public static SwerveModuleIO createModule(SwerveModuleConfig config) { - return new SwerveModuleIOCustom(config); + private static SwerveModuleIO createModule(CAN steer, CAN azimuth, CAN drive, Rotation2d offset) { + return new SwerveModuleIOCustom(steer, azimuth, drive, offset); + } + + /** + * creates the north west swerve module. + * + * @return the north west swerve module. + */ + public static SwerveModuleIO createNorthWestModule() { + return createModule( + new CAN(8, "swerve"), + new CAN(16, "swerve"), + new CAN(24, "swerve"), + Rotation2d.fromRotations(-0.084717).unaryMinus()); + } + + /** + * Creates the north west swerve module translation. + * + * @return the north west swerve module translation. + */ + public static Translation2d createNorthWestModuleTranslation() { + return new Translation2d(Units.inchesToMeters(10.375), Units.inchesToMeters(10.375)); + } + + /** + * creates the north east swerve module. + * + * @return the north east swerve module. + */ + public static SwerveModuleIO createNorthEastModule() { + return createModule( + new CAN(16, "swerve"), + new CAN(18, "swerve"), + new CAN(30, "swerve"), + Rotation2d.fromRotations(0.196777).unaryMinus()); + } + + /** + * Creates the north east swerve module translation. + * + * @return the north east swerve module translation. + */ + public static Translation2d createNorthEastModuleTranslation() { + return new Translation2d(Units.inchesToMeters(10.375), Units.inchesToMeters(-10.375)); + } + + /** + * creates the south east swerve module. + * + * @return the south east swerve module. + */ + public static SwerveModuleIO createSouthEastModule() { + return createModule( + new CAN(12, "swerve"), + new CAN(22, "swerve"), + new CAN(26, "swerve"), + Rotation2d.fromRotations(0.276611).unaryMinus()); + } + + /** + * Creates the south east swerve module translation. + * + * @return the south east swerve module translation. + */ + public static Translation2d createSouthEastModuleTranslation() { + return new Translation2d(Units.inchesToMeters(-10.375), Units.inchesToMeters(-10.375)); + } + + /** + * creates the south west swerve module. + * + * @return the south west swerve module. + */ + public static SwerveModuleIO createSouthWestModule() { + return createModule( + new CAN(10, "swerve"), + new CAN(20, "swerve"), + new CAN(28, "drive"), + Rotation2d.fromRotations(0.223145).unaryMinus()); + } + + /** + * Creates the south west swerve module translation. + * + * @return the south west swerve module translation. + */ + public static Translation2d createSouthWestModuleTranslation() { + return new Translation2d(Units.inchesToMeters(-10.375), Units.inchesToMeters(10.375)); } /** @@ -49,8 +139,7 @@ public static PositionControllerIO createSteerMotor(CAN steer, CAN azimuth, Rota */ public static VelocityControllerIO createDriveMotor(CAN drive) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SWERVE)) - return new VelocityControllerIOTalonFXPIDF( - drive, SwerveConstants.DRIVE_CONFIG, false); + return new VelocityControllerIOTalonFXPIDF(drive, SwerveConstants.DRIVE_CONFIG, false); return new VelocityControllerIOSim(); } diff --git a/src/main/java/frc/robot/swerve/SwerveModuleCAN.java b/src/main/java/frc/robot/swerve/SwerveModuleCAN.java deleted file mode 100644 index 4f9fd1b..0000000 --- a/src/main/java/frc/robot/swerve/SwerveModuleCAN.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.swerve; - -import frc.lib.CAN; -import java.util.Objects; - -/** CAN identifiers for a swerve module. */ -public record SwerveModuleCAN(CAN azimuth, CAN steer, CAN drive) { - - /** - * Creates the CAN identifiers for a swerve module. - * - * @param azimuth the CAN identifier for the swerve module's azimuth encoder. - * @param steer the CAN identifier for the swerve module's steer motor. - * @param drive the CAN identifier for the swerve module's drive motor. - */ - public SwerveModuleCAN { - Objects.requireNonNull(azimuth); - Objects.requireNonNull(steer); - Objects.requireNonNull(drive); - } - - /** - * Creates the CAN identifier for a swerve module using numeric identifiers. - * - * @param azimuth the numeric identifier assigned to the swerve module's azimuth encoder. - * @param steer the numeric identifier assigned to the swerve module's steer motor. - * @param drive the numeric identifier assigned to the swerve module's drive motor. - * @param bus the CAN bus that the swerve module's devices are located on. - */ - public SwerveModuleCAN(int azimuth, int steer, int drive, String bus) { - this(new CAN(azimuth, bus), new CAN(steer, bus), new CAN(drive, bus)); - } -} diff --git a/src/main/java/frc/robot/swerve/SwerveModuleConfig.java b/src/main/java/frc/robot/swerve/SwerveModuleConfig.java deleted file mode 100644 index 11f19a3..0000000 --- a/src/main/java/frc/robot/swerve/SwerveModuleConfig.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.swerve; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import java.util.Objects; - -/** Configuration of a swerve module. */ -public record SwerveModuleConfig( - SwerveModuleCAN moduleCAN, Translation2d position, Rotation2d offset) { - - /** - * Creates the configuration of a swerve module. - * - * @param moduleCAN the swerve module's CAN identifiers. - * @param position the swerve module's position relative to the center of the robot. - * @param offset the swerve module steer motor's offset. - */ - public SwerveModuleConfig { - Objects.requireNonNull(moduleCAN); - Objects.requireNonNull(position); - Objects.requireNonNull(offset); - } -} diff --git a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java index 0aba5c3..05e2651 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import frc.lib.CAN; import frc.lib.controller.PositionControllerIO; import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; import frc.lib.controller.VelocityControllerIO; @@ -28,16 +29,11 @@ public class SwerveModuleIOCustom implements SwerveModuleIO { /** Module setpoint */ private SwerveModuleState setpoint; - /** - * Creates a custom swerve module. - * - * @param config the swerve module's configuration. - */ - public SwerveModuleIOCustom(SwerveModuleConfig config) { - steerMotor = SwerveFactory.createSteerMotor(config.moduleCAN().steer(), config.moduleCAN().azimuth(), config.offset()); + public SwerveModuleIOCustom(CAN steer, CAN azimuth, CAN drive, Rotation2d offset) { + steerMotor = SwerveFactory.createSteerMotor(steer, azimuth, offset); steerMotor.configure(); - driveMotor = SwerveFactory.createDriveMotor(config.moduleCAN().drive()); + driveMotor = SwerveFactory.createDriveMotor(drive); driveMotor.configure(); setpoint = new SwerveModuleState();