diff --git a/src/main/java/frc/robot/swerve/AzimuthEncoderIO.java b/src/main/java/frc/robot/swerve/AzimuthEncoderIO.java index a47f7d4..8ef6b47 100644 --- a/src/main/java/frc/robot/swerve/AzimuthEncoderIO.java +++ b/src/main/java/frc/robot/swerve/AzimuthEncoderIO.java @@ -3,17 +3,16 @@ /** Azimuth encoder hardware interface. */ public interface AzimuthEncoderIO { - /** Values for the azimuth encoder hardware interface. */ - public static class AzimuthEncoderIOValues { - /** Position of the azimuth encoder in rotations. */ - public double angleRotations = 0.0; - } + /** Values for the azimuth encoder hardware interface. */ + public static class AzimuthEncoderIOValues { + /** Position of the azimuth encoder in rotations. */ + public double angleRotations = 0.0; + } - /** - * Updates the azimuth encoder's values. - * - * @param values - */ - public void update(AzimuthEncoderIOValues values); - + /** + * Updates the azimuth encoder's values. + * + * @param values + */ + public void update(AzimuthEncoderIOValues values); } diff --git a/src/main/java/frc/robot/swerve/AzimuthEncoderIOSim.java b/src/main/java/frc/robot/swerve/AzimuthEncoderIOSim.java index 2d72525..c10fd40 100644 --- a/src/main/java/frc/robot/swerve/AzimuthEncoderIOSim.java +++ b/src/main/java/frc/robot/swerve/AzimuthEncoderIOSim.java @@ -3,9 +3,8 @@ /** Simulated azimuth encoder. */ public class AzimuthEncoderIOSim implements AzimuthEncoderIO { - @Override - public void update(AzimuthEncoderIOValues values) { - values.angleRotations = 0.0; - } - + @Override + public void update(AzimuthEncoderIOValues values) { + values.angleRotations = 0.0; + } } diff --git a/src/main/java/frc/robot/swerve/DriveMotorIO.java b/src/main/java/frc/robot/swerve/DriveMotorIO.java index f080387..7d58c1b 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIO.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIO.java @@ -3,33 +3,33 @@ /** Drive motor hardware interface. */ public interface DriveMotorIO { - /** Values for the drive motor hardware interface. */ - public static class DriveMotorIOValues { - /** Position of the drive motor in meters. */ - public double positionMeters = 0.0; + /** Values for the drive motor hardware interface. */ + public static class DriveMotorIOValues { + /** Position of the drive motor in meters. */ + public double positionMeters = 0.0; - /** Velocity of the drive motor in meters per second. */ - public double velocityMetersPerSecond = 0.0; - } + /** Velocity of the drive motor in meters per second. */ + public double velocityMetersPerSecond = 0.0; + } - /** - * Updates the drive motor's values. - * - * @param values - */ - public void update(DriveMotorIOValues values); + /** + * Updates the drive motor's values. + * + * @param values + */ + public void update(DriveMotorIOValues values); - /** - * Sets the drive motor's position. - * - * @param positionMeters the drive motor's position. - */ - public void setPosition(double positionMeters); + /** + * Sets the drive motor's position. + * + * @param positionMeters the drive motor's position. + */ + public void setPosition(double positionMeters); - /** - * Sets the drive motor's setpoint. - * - * @param velocityMetersPerSecond the drive motor's setpoint. - */ - public void setSetpoint(double velocityMetersPerSecond); + /** + * Sets the drive motor's setpoint. + * + * @param velocityMetersPerSecond the drive motor's setpoint. + */ + public void setSetpoint(double velocityMetersPerSecond); } diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOSim.java b/src/main/java/frc/robot/swerve/DriveMotorIOSim.java index f70c949..45e0a12 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOSim.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOSim.java @@ -5,28 +5,27 @@ /** Simulated drive motor. */ public class DriveMotorIOSim implements DriveMotorIO { - /** Represents the position of the drive motor. */ - private double positionMeters; + /** Represents the position of the drive motor. */ + private double positionMeters; - /** Represents the velocity of the drive motor. */ - private double velocityMetersPerSecond; + /** Represents the velocity of the drive motor. */ + private double velocityMetersPerSecond; - @Override - public void update(DriveMotorIOValues values) { - this.positionMeters += velocityMetersPerSecond * RobotConstants.TICK_PERIOD; + @Override + public void update(DriveMotorIOValues values) { + this.positionMeters += velocityMetersPerSecond * RobotConstants.TICK_PERIOD; - values.positionMeters = positionMeters; - values.velocityMetersPerSecond = velocityMetersPerSecond; - } + values.positionMeters = positionMeters; + values.velocityMetersPerSecond = velocityMetersPerSecond; + } - @Override - public void setPosition(double positionMeters) { - this.positionMeters = positionMeters; - } - - @Override - public void setSetpoint(double velocityMetersPerSecond) { - this.velocityMetersPerSecond = velocityMetersPerSecond; - } + @Override + public void setPosition(double positionMeters) { + this.positionMeters = positionMeters; + } + @Override + public void setSetpoint(double velocityMetersPerSecond) { + this.velocityMetersPerSecond = velocityMetersPerSecond; + } } diff --git a/src/main/java/frc/robot/swerve/SteerMotorIO.java b/src/main/java/frc/robot/swerve/SteerMotorIO.java index a3ebe70..3b9c1c1 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIO.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIO.java @@ -3,33 +3,33 @@ /** Steer motor hardware interface. */ public interface SteerMotorIO { - /** Values for the steer motor hardware interface. */ - public static class SteerMotorIOValues { - /** Position of the steer motor in rotations. */ - public double angleRotations = 0.0; - /** Velocity of the steer motor in rotations per second. */ - public double omegaRotationsPerSecond = 0.0; - } + /** Values for the steer motor hardware interface. */ + public static class SteerMotorIOValues { + /** Position of the steer motor in rotations. */ + public double angleRotations = 0.0; - /** - * Updates the steer motor's values. - * - * @param values - */ - public void update(SteerMotorIOValues values); + /** Velocity of the steer motor in rotations per second. */ + public double omegaRotationsPerSecond = 0.0; + } - /** - * Sets the steer motor's position. - * - * @param angleRotations the steer motor's position. - */ - public void setPosition(double angleRotations); + /** + * Updates the steer motor's values. + * + * @param values + */ + public void update(SteerMotorIOValues values); - /** - * Sets the steer motor's setpoint. - * - * @param angleRotations the steer motor's setpoint. - */ - public void setSetpoint(double angleRotations); - + /** + * Sets the steer motor's position. + * + * @param angleRotations the steer motor's position. + */ + public void setPosition(double angleRotations); + + /** + * Sets the steer motor's setpoint. + * + * @param angleRotations the steer motor's setpoint. + */ + public void setSetpoint(double angleRotations); } diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOSim.java b/src/main/java/frc/robot/swerve/SteerMotorIOSim.java index bd55023..9d6a805 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOSim.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOSim.java @@ -9,48 +9,46 @@ /** Simulated steer motor. */ public class SteerMotorIOSim implements SteerMotorIO { - /** Represents the motor used to steer the wheel. */ - private final DCMotor motorSim = DCMotor.getFalcon500(1); // TODO + /** Represents the motor used to steer the wheel. */ + private final DCMotor motorSim = DCMotor.getFalcon500(1); // TODO - /** Represents the wheel steered by the motor. */ - private final FlywheelSim wheelSim = new FlywheelSim(motorSim, MK4iConstants.STEER_GEARING, MK4iConstants.STEER_MOI); + /** Represents the wheel steered by the motor. */ + private final FlywheelSim wheelSim = + new FlywheelSim(motorSim, MK4iConstants.STEER_GEARING, MK4iConstants.STEER_MOI); - /** Represents the angle of the steer motor. */ - private double angleRotations; + /** Represents the angle of the steer motor. */ + private double angleRotations; - /** Feedback controller for the angle. */ - private final PIDController angleController = new PIDController(1.0, 0, 0); + /** Feedback controller for the angle. */ + private final PIDController angleController = new PIDController(1.0, 0, 0); - public SteerMotorIOSim() { - angleRotations = 0.0; + public SteerMotorIOSim() { + angleRotations = 0.0; - angleController.enableContinuousInput(0, 1); - } + angleController.enableContinuousInput(0, 1); + } - @Override - public void update(SteerMotorIOValues values) { - double voltage = angleController.calculate(angleRotations); + @Override + public void update(SteerMotorIOValues values) { + double voltage = angleController.calculate(angleRotations); - wheelSim.setInputVoltage(voltage); - wheelSim.update(RobotConstants.TICK_PERIOD); + wheelSim.setInputVoltage(voltage); + wheelSim.update(RobotConstants.TICK_PERIOD); - double omegaRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0; - angleRotations += omegaRotationsPerSecond; + double omegaRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0; + angleRotations += omegaRotationsPerSecond; - values.angleRotations = angleRotations; - values.omegaRotationsPerSecond = omegaRotationsPerSecond; - } - - @Override - public void setPosition(double angleRotations) { - this.angleRotations = 0.0; - } - - @Override - public void setSetpoint(double angleRotations) { - angleController.setSetpoint(angleRotations); - } - + values.angleRotations = angleRotations; + values.omegaRotationsPerSecond = omegaRotationsPerSecond; + } + @Override + public void setPosition(double angleRotations) { + this.angleRotations = 0.0; + } + @Override + public void setSetpoint(double angleRotations) { + angleController.setSetpoint(angleRotations); + } } diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 16a2516..66e5f83 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -20,7 +20,7 @@ public class Swerve extends Subsystem { /** Swerve modules controlled by the swerve subsystem. */ private final SwerveModuleIO[] swerveModules = new SwerveModuleIO[4]; - /** Swerve kinematics. */ + /** Swerve kinematics. */ private final SwerveDriveKinematics swerveKinematics; /** Creates a new instance of the swerve subsystem. */ @@ -30,7 +30,12 @@ private Swerve() { swerveModules[2] = SwerveFactory.createModule(SwerveConstants.SOUTH_EAST_MODULE_CONFIG); swerveModules[3] = SwerveFactory.createModule(SwerveConstants.SOUTH_WEST_MODULE_CONFIG); - 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()); + 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()); } /** @@ -53,28 +58,32 @@ public void periodic() {} public void addToShuffleboard(ShuffleboardTab tab) { ShuffleboardLayout northWestModule = Telemetry.addColumn(tab, "North West Module"); - northWestModule.addDouble("Angle (deg)", () -> swerveModules[0].getPosition().angle.getDegrees()); + northWestModule.addDouble( + "Angle (deg)", () -> swerveModules[0].getPosition().angle.getDegrees()); northWestModule.addDouble("Distance (m)", () -> swerveModules[0].getPosition().distanceMeters); ShuffleboardLayout northEastModule = Telemetry.addColumn(tab, "North East Module"); - northEastModule.addDouble("Angle (deg)", () -> swerveModules[1].getPosition().angle.getDegrees()); + northEastModule.addDouble( + "Angle (deg)", () -> swerveModules[1].getPosition().angle.getDegrees()); northEastModule.addDouble("Distance (m)", () -> swerveModules[1].getPosition().distanceMeters); ShuffleboardLayout southEastModule = Telemetry.addColumn(tab, "South East Module"); - southEastModule.addDouble("Angle (deg)", () -> swerveModules[2].getPosition().angle.getDegrees()); + southEastModule.addDouble( + "Angle (deg)", () -> swerveModules[2].getPosition().angle.getDegrees()); southEastModule.addDouble("Distance (m)", () -> swerveModules[2].getPosition().distanceMeters); ShuffleboardLayout southWestModule = Telemetry.addColumn(tab, "South West Module"); - southWestModule.addDouble("Angle (deg)", () -> swerveModules[3].getPosition().angle.getDegrees()); + southWestModule.addDouble( + "Angle (deg)", () -> swerveModules[3].getPosition().angle.getDegrees()); southWestModule.addDouble("Distance (m)", () -> swerveModules[3].getPosition().distanceMeters); } /** * Sets the swerve's speeds. - * + * * @param speeds the swerve's speeds. */ public void setChassisSpeeds(ChassisSpeeds speeds) { @@ -84,8 +93,8 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { } /** - * Set the setpoints for each of the swerve's modules. - * + * Set the setpoints for each of the swerve's modules. + * * @param setpoints the setpoints for each of the swerve's modules. * @param lazy if true, optimize the module setpoint. */ @@ -99,36 +108,41 @@ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { /** * Orients all swerve modules. - * + * * @param orientations orientations for each swerve modules. * @return a command that orients all swerve modules. */ public Command orientModules(Rotation2d[] orientations) { - return Commands.run(() -> { - setSetpoints(new SwerveModuleState[] { - new SwerveModuleState(0.0, orientations[0]), - new SwerveModuleState(0.0, orientations[1]), - new SwerveModuleState(0.0, orientations[2]), - new SwerveModuleState(0.0, orientations[3]), - }, false); - }); + return Commands.run( + () -> { + setSetpoints( + new SwerveModuleState[] { + new SwerveModuleState(0.0, orientations[0]), + new SwerveModuleState(0.0, orientations[1]), + new SwerveModuleState(0.0, orientations[2]), + new SwerveModuleState(0.0, orientations[3]), + }, + false); + }); } public Command forwards() { - return orientModules(new Rotation2d[] { - Rotation2d.fromDegrees(0.0), - Rotation2d.fromDegrees(0.0), - Rotation2d.fromDegrees(0.0), - Rotation2d.fromDegrees(0.0) - }); + return orientModules( + new Rotation2d[] { + Rotation2d.fromDegrees(0.0), + Rotation2d.fromDegrees(0.0), + Rotation2d.fromDegrees(0.0), + Rotation2d.fromDegrees(0.0) + }); } public Command sideways() { - return orientModules(new Rotation2d[] { - Rotation2d.fromDegrees(90.0), - Rotation2d.fromDegrees(90.0), - Rotation2d.fromDegrees(90.0), - Rotation2d.fromDegrees(90.0) - }); + return orientModules( + new Rotation2d[] { + Rotation2d.fromDegrees(90.0), + Rotation2d.fromDegrees(90.0), + Rotation2d.fromDegrees(90.0), + Rotation2d.fromDegrees(90.0) + }); } } diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 5995877..c039e4c 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -7,27 +7,35 @@ /** Constants for the swerve subsystem. */ public class SwerveConstants { - /** Constants for the MK4i COTS module. */ - public static class MK4iConstants { - /** Gearing between the steer motor and the wheel. */ - public static final double STEER_GEARING = 150.0 / 7.0; - - /** Moment of inertia of the wheel when steering in joules kilograms meters squared. */ - public static final double STEER_MOI = 0.004; // TODO - } - - /** Module configuration for the north west swerve module. */ - public static final SwerveModuleConfig NORTH_WEST_MODULE_CONFIG = new SwerveModuleConfig(new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO - - /** Module configuration for the north east swerve module. */ - public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG = new SwerveModuleConfig(new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO - - /** Module configuration for the south east swerve module. */ - public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = new SwerveModuleConfig(new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO - - /** Module configuration for the south west swerve module. */ - public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG = new SwerveModuleConfig(new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO - - /** Maximum attainable speed in meters per second. */ - public static final double MAXIMUM_SPEED = Units.feetToMeters(4.0); + /** Constants for the MK4i COTS module. */ + public static class MK4iConstants { + /** Gearing between the steer motor and the wheel. */ + public static final double STEER_GEARING = 150.0 / 7.0; + + /** Moment of inertia of the wheel when steering in joules kilograms meters squared. */ + public static final double STEER_MOI = 0.004; // TODO + } + + /** Module configuration for the north west swerve module. */ + public static final SwerveModuleConfig NORTH_WEST_MODULE_CONFIG = + new SwerveModuleConfig( + new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO + + /** Module configuration for the north east swerve module. */ + public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG = + new SwerveModuleConfig( + new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO + + /** Module configuration for the south east swerve module. */ + public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = + new SwerveModuleConfig( + new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO + + /** Module configuration for the south west swerve module. */ + public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG = + new SwerveModuleConfig( + new SwerveModuleCAN(0, 0, 0, ""), new Translation2d(), new Rotation2d()); // TODO + + /** Maximum attainable speed in meters per second. */ + public static final double MAXIMUM_SPEED = Units.feetToMeters(4.0); } diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index 7acaae2..4563ea8 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -5,48 +5,47 @@ /** Helper class for creating hardware for the swerve subsystem. */ public class SwerveFactory { - /** - * Creates a swerve module. - * - * @return a swerve module. - */ - public static SwerveModuleIO createModule(SwerveModuleConfig config) { - if (Robot.isReal()) return new SwerveModuleIOCustom(); // TODO - - return new SwerveModuleIOCustom(); - } - - /** - * Creates an azimuth encoder. - * - * @return an azimuth encoder. - */ - public static AzimuthEncoderIO createAzimuthEncoder() { - if (Robot.isReal()) return new AzimuthEncoderIOSim(); - - return new AzimuthEncoderIOSim(); - } - - /** - * Creates a steer motor. - * - * @return a steer motor. - */ - public static SteerMotorIO createSteerMotor() { - if (Robot.isReal()) return new SteerMotorIOSim(); - - return new SteerMotorIOSim(); - } - - /** - * Creates a drive motor. - * - * @return a drive motor. - */ - public static DriveMotorIO createDriveMotor() { - if (Robot.isReal()) return new DriveMotorIOSim(); - - return new DriveMotorIOSim(); - } - + /** + * Creates a swerve module. + * + * @return a swerve module. + */ + public static SwerveModuleIO createModule(SwerveModuleConfig config) { + if (Robot.isReal()) return new SwerveModuleIOCustom(); // TODO + + return new SwerveModuleIOCustom(); + } + + /** + * Creates an azimuth encoder. + * + * @return an azimuth encoder. + */ + public static AzimuthEncoderIO createAzimuthEncoder() { + if (Robot.isReal()) return new AzimuthEncoderIOSim(); + + return new AzimuthEncoderIOSim(); + } + + /** + * Creates a steer motor. + * + * @return a steer motor. + */ + public static SteerMotorIO createSteerMotor() { + if (Robot.isReal()) return new SteerMotorIOSim(); + + return new SteerMotorIOSim(); + } + + /** + * Creates a drive motor. + * + * @return a drive motor. + */ + public static DriveMotorIO createDriveMotor() { + if (Robot.isReal()) return new DriveMotorIOSim(); + + return new DriveMotorIOSim(); + } } diff --git a/src/main/java/frc/robot/swerve/SwerveModuleCAN.java b/src/main/java/frc/robot/swerve/SwerveModuleCAN.java index 5bd61c3..4f9fd1b 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleCAN.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleCAN.java @@ -1,18 +1,17 @@ package frc.robot.swerve; -import java.util.Objects; - 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. + * @param drive the CAN identifier for the swerve module's drive motor. */ public SwerveModuleCAN { Objects.requireNonNull(azimuth); @@ -21,8 +20,8 @@ public record SwerveModuleCAN(CAN azimuth, CAN steer, CAN drive) { } /** - * Creates the CAN identifier for a swerve module using numeric identifiers. - * + * 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. @@ -31,5 +30,4 @@ public record SwerveModuleCAN(CAN azimuth, CAN steer, CAN drive) { 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 index 6fd475d..e6027bf 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleConfig.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleConfig.java @@ -1,24 +1,23 @@ package frc.robot.swerve; -import java.util.Objects; - 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. - * @param offset the swerve module steer motor's offset. - */ - public SwerveModuleConfig { - Objects.requireNonNull(moduleCAN); - Objects.requireNonNull(position); - Objects.requireNonNull(offset); - } +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. + * @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/SwerveModuleIO.java b/src/main/java/frc/robot/swerve/SwerveModuleIO.java index ca441cf..122bd6e 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIO.java @@ -6,27 +6,25 @@ /** Swerve module hardware interface. */ public interface SwerveModuleIO { - /** - * Sets the swerve module setpoint. - * - * @param setpoint the swerve module setpoint. - * @param lazy if true, optimize the swerve module setpoint. - */ - public void setSetpoint(SwerveModuleState setpoint, boolean lazy); + /** + * Sets the swerve module setpoint. + * + * @param setpoint the swerve module setpoint. + * @param lazy if true, optimize the swerve module setpoint. + */ + public void setSetpoint(SwerveModuleState setpoint, boolean lazy); - /** - * Gets the state of the swerve module. - * - * @return the state of the swerve module. - */ - public SwerveModuleState getState(); - - /** - * Gets the position of the swerve module. - * - * @return the position of the swerve module. - */ - public SwerveModulePosition getPosition(); + /** + * Gets the state of the swerve module. + * + * @return the state of the swerve module. + */ + public SwerveModuleState getState(); + /** + * Gets the position of the swerve module. + * + * @return the position of the swerve module. + */ + public SwerveModulePosition getPosition(); } - diff --git a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java index 8edae54..364e17d 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java @@ -9,69 +9,71 @@ public class SwerveModuleIOCustom implements SwerveModuleIO { - /** Azimuth encoder. */ - private final AzimuthEncoderIO azimuthEncoder; - - /** Azimuth encoder values. */ - private final AzimuthEncoderIOValues azimuthEncoderValues = new AzimuthEncoderIOValues(); - - /** Steer motor. */ - private final SteerMotorIO steerMotor; - - /** Steer motor values. */ - private final SteerMotorIOValues steerMotorValues = new SteerMotorIOValues(); - - /** Drive motor. */ - private final DriveMotorIO driveMotor; - - /** Driver motor values. */ - private final DriveMotorIOValues driveMotorValues = new DriveMotorIOValues(); - - public SwerveModuleIOCustom() { - azimuthEncoder = SwerveFactory.createAzimuthEncoder(); - steerMotor = SwerveFactory.createSteerMotor(); - driveMotor = SwerveFactory.createDriveMotor(); - - azimuthEncoder.update(azimuthEncoderValues); - steerMotor.setPosition(azimuthEncoderValues.angleRotations); - } + /** Azimuth encoder. */ + private final AzimuthEncoderIO azimuthEncoder; - @Override - public void setSetpoint(SwerveModuleState setpoint, boolean lazy) { - if (lazy) { - setpoint = optimize(setpoint); - } + /** Azimuth encoder values. */ + private final AzimuthEncoderIOValues azimuthEncoderValues = new AzimuthEncoderIOValues(); - steerMotor.setSetpoint(setpoint.angle.getRotations()); - driveMotor.setSetpoint(setpoint.speedMetersPerSecond); - } + /** Steer motor. */ + private final SteerMotorIO steerMotor; - /** - * Optimizes a swerve module's setpoint. - * - * @param setpoint the setpoint to optimize. - * @return the optimized setpoint. - */ - private SwerveModuleState optimize(SwerveModuleState setpoint) { - return setpoint; // TODO - } + /** Steer motor values. */ + private final SteerMotorIOValues steerMotorValues = new SteerMotorIOValues(); - @Override - public SwerveModuleState getState() { - azimuthEncoder.update(azimuthEncoderValues); - steerMotor.update(steerMotorValues); - driveMotor.update(driveMotorValues); + /** Drive motor. */ + private final DriveMotorIO driveMotor; - return new SwerveModuleState(driveMotorValues.velocityMetersPerSecond, Rotation2d.fromRotations(steerMotorValues.angleRotations)); - } + /** Driver motor values. */ + private final DriveMotorIOValues driveMotorValues = new DriveMotorIOValues(); + + public SwerveModuleIOCustom() { + azimuthEncoder = SwerveFactory.createAzimuthEncoder(); + steerMotor = SwerveFactory.createSteerMotor(); + driveMotor = SwerveFactory.createDriveMotor(); - @Override - public SwerveModulePosition getPosition() { - azimuthEncoder.update(azimuthEncoderValues); - steerMotor.update(steerMotorValues); - driveMotor.update(driveMotorValues); + azimuthEncoder.update(azimuthEncoderValues); + steerMotor.setPosition(azimuthEncoderValues.angleRotations); + } - return new SwerveModulePosition(driveMotorValues.positionMeters, Rotation2d.fromRotations(steerMotorValues.angleRotations)); + @Override + public void setSetpoint(SwerveModuleState setpoint, boolean lazy) { + if (lazy) { + setpoint = optimize(setpoint); } - + + steerMotor.setSetpoint(setpoint.angle.getRotations()); + driveMotor.setSetpoint(setpoint.speedMetersPerSecond); + } + + /** + * Optimizes a swerve module's setpoint. + * + * @param setpoint the setpoint to optimize. + * @return the optimized setpoint. + */ + private SwerveModuleState optimize(SwerveModuleState setpoint) { + return setpoint; // TODO + } + + @Override + public SwerveModuleState getState() { + azimuthEncoder.update(azimuthEncoderValues); + steerMotor.update(steerMotorValues); + driveMotor.update(driveMotorValues); + + return new SwerveModuleState( + driveMotorValues.velocityMetersPerSecond, + Rotation2d.fromRotations(steerMotorValues.angleRotations)); + } + + @Override + public SwerveModulePosition getPosition() { + azimuthEncoder.update(azimuthEncoderValues); + steerMotor.update(steerMotorValues); + driveMotor.update(driveMotorValues); + + return new SwerveModulePosition( + driveMotorValues.positionMeters, Rotation2d.fromRotations(steerMotorValues.angleRotations)); + } }