Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
refactor(swerve): Do away with module config.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 28, 2024
1 parent ee05045 commit 7716544
Show file tree
Hide file tree
Showing 7 changed files with 108 additions and 110 deletions.
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/auto/Auto.java
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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);
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/swerve/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

/**
Expand Down
32 changes: 0 additions & 32 deletions src/main/java/frc/robot/swerve/SwerveConstants.java
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -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;

Expand Down
99 changes: 94 additions & 5 deletions src/main/java/frc/robot/swerve/SwerveFactory.java
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -13,16 +15,104 @@
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 {

/**
* Creates a swerve module.
*
* @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));
}

/**
Expand All @@ -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();
}
Expand Down
33 changes: 0 additions & 33 deletions src/main/java/frc/robot/swerve/SwerveModuleCAN.java

This file was deleted.

23 changes: 0 additions & 23 deletions src/main/java/frc/robot/swerve/SwerveModuleConfig.java

This file was deleted.

12 changes: 4 additions & 8 deletions src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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();
Expand Down

0 comments on commit 7716544

Please sign in to comment.