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

Commit

Permalink
refactor(swerve): Move swerve constants out of library.
Browse files Browse the repository at this point in the history
  • Loading branch information
@haydenheroux
haydenheroux committed Apr 28, 2024
1 parent b8dd291 commit 66d88f5
Show file tree
Hide file tree
Showing 4 changed files with 47 additions and 66 deletions.
18 changes: 9 additions & 9 deletions src/main/java/frc/lib/controller/SwerveModuleIOCustom.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,8 @@
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.config.MechanismConfig;
import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
import frc.robot.swerve.SwerveFactory;

/** Custom swerve module. */
public class SwerveModuleIOCustom implements SwerveModuleIO {
Expand All @@ -25,18 +22,21 @@ public class SwerveModuleIOCustom implements SwerveModuleIO {
/** Drive motor values. */
private final VelocityControllerIOValues driveMotorValues = new VelocityControllerIOValues();

private final double wheelCircumference = Units.inchesToMeters(4.0) * Math.PI;
/** Wheel circumference. */
private final double wheelCircumference;

/** Module setpoint */
private SwerveModuleState setpoint;

public SwerveModuleIOCustom(
CAN steer, CAN azimuth, CAN drive, MechanismConfig steerConfig, MechanismConfig driveConfig) {
steerMotor = SwerveFactory.createSteerMotor(steer, azimuth, steerConfig);
steerMotor.configure();
PositionControllerIO steerMotor, VelocityControllerIO driveMotor, double wheelCircumference) {
this.steerMotor = steerMotor;
this.steerMotor.configure();

driveMotor = SwerveFactory.createDriveMotor(drive, driveConfig);
driveMotor.configure();
this.driveMotor = driveMotor;
this.driveMotor.configure();

this.wheelCircumference = wheelCircumference;

setpoint = new SwerveModuleState();
}
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/auto/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
import frc.robot.odometry.Odometry;
import frc.robot.superstructure.Superstructure;
import frc.robot.swerve.Swerve;
import frc.robot.swerve.SwerveFactory;

/** Auto subsystem. */
public class Auto extends Subsystem {
Expand Down Expand Up @@ -49,7 +48,7 @@ private Auto() {
new PIDConstants(1.0),
new PIDConstants(1.0),
swerve.maximumTranslationVelocity(),
SwerveFactory.createNorthEastModuleTranslation().getNorm(), // TODO
swerve.driveRadius(),
new ReplanningConfig()),
AllianceFlipHelper::shouldFlip,
swerve);
Expand Down
60 changes: 15 additions & 45 deletions src/main/java/frc/robot/swerve/Swerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,9 @@ public class Swerve extends Subsystem {
.withProportionalGain(0.75) // volts per rotation per second
);

/** Wheel circumference. */
private final double wheelCircumference = Units.inchesToMeters(4.0) * Math.PI;

/** Translation motion profile config. */
private final MotionProfileConfig translationMotionProfileConfig =
new MotionProfileConfig()
Expand All @@ -85,10 +88,14 @@ public class Swerve extends Subsystem {

/** Initializes the swerve subsystem and configures swerve hardware. */
private Swerve() {
swerveModules[0] = SwerveFactory.createNorthWestModule(steerConfig, driveConfig);
swerveModules[1] = SwerveFactory.createNorthEastModule(steerConfig, driveConfig);
swerveModules[2] = SwerveFactory.createSouthEastModule(steerConfig, driveConfig);
swerveModules[3] = SwerveFactory.createSouthWestModule(steerConfig, driveConfig);
swerveModules[0] =
SwerveFactory.createNorthWestModule(steerConfig, driveConfig, wheelCircumference);
swerveModules[1] =
SwerveFactory.createNorthEastModule(steerConfig, driveConfig, wheelCircumference);
swerveModules[2] =
SwerveFactory.createSouthEastModule(steerConfig, driveConfig, wheelCircumference);
swerveModules[3] =
SwerveFactory.createSouthWestModule(steerConfig, driveConfig, wheelCircumference);

swerveKinematics =
new SwerveDriveKinematics(
Expand Down Expand Up @@ -116,16 +123,6 @@ public void periodic() {}

@Override
public void addToShuffleboard(ShuffleboardTab tab) {
ShuffleboardLayout translationConstants = Telemetry.addColumn(tab, "Translation Constants");

translationConstants.addDouble("Maximum Velocity (mps)", this::maximumTranslationVelocity);
translationConstants.addDouble(
"Maximum Accleration (mpsps)", this::maximumTranslationAcceleration);

ShuffleboardLayout rotationConstants = Telemetry.addColumn(tab, "Rotation Constants");
rotationConstants.addDouble(
"Maximum Velocity (rps)", () -> maximumRotationVelocity().getRotations());

Telemetry.addSwerveModuleStates(tab, "Swerve Module States", this::getModuleStates);
Telemetry.addSwerveModuleStates(tab, "Swerve Module Setpoints", this::getModuleSetpoints);

Expand Down Expand Up @@ -234,15 +231,6 @@ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) {
}
}

/**
* Returns the motion profile config.
*
* @return the motion profile config.
*/
public MotionProfileConfig translationMotionProfileConfig() {
return translationMotionProfileConfig;
}

/**
* Returns the maximum translation velocity.
*
Expand All @@ -253,30 +241,12 @@ public double maximumTranslationVelocity() {
}

/**
* Returns the maximum translation acceleration.
*
* @return the maximum translation acceleration.
*/
public double maximumTranslationAcceleration() {
return translationMotionProfileConfig.maximumAcceleration();
}

/**
* Returns the rotation motion profile config.
*
* @return the rotation motion profile config.
*/
public MotionProfileConfig rotationMotionProfileConfig() {
return rotationMotionProfileConfig;
}

/**
* Returns the maximum rotation velocity.
* Returns the drive radius (distance to the farthest module).
*
* @return the maximum rotation velocity.
* @return the drive readius (distance to the farthest module).
*/
public Rotation2d maximumRotationVelocity() {
return Rotation2d.fromRotations(rotationMotionProfileConfig.maximumVelocity());
public double driveRadius() {
return SwerveFactory.createNorthEastModuleTranslation().getNorm();
}

/**
Expand Down
32 changes: 22 additions & 10 deletions src/main/java/frc/robot/swerve/SwerveFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,16 @@ public class SwerveFactory {
* @return a swerve module.
*/
private static SwerveModuleIO createModule(
CAN steer, CAN azimuth, CAN drive, MechanismConfig steerConfig, MechanismConfig driveConfig) {
return new SwerveModuleIOCustom(steer, azimuth, drive, steerConfig, driveConfig);
CAN steer,
CAN azimuth,
CAN drive,
MechanismConfig steerConfig,
MechanismConfig driveConfig,
double wheelCircumference) {
return new SwerveModuleIOCustom(
createSteerMotor(steer, azimuth, steerConfig),
createDriveMotor(drive, driveConfig),
wheelCircumference);
}

/**
Expand All @@ -37,15 +45,16 @@ private static SwerveModuleIO createModule(
* @return the north west swerve module.
*/
public static SwerveModuleIO createNorthWestModule(
MechanismConfig steerConfig, MechanismConfig driveConfig) {
MechanismConfig steerConfig, MechanismConfig driveConfig, double wheelCircumference) {
return createModule(
new CAN(8, "swerve"),
new CAN(16, "swerve"),
new CAN(24, "swerve"),
steerConfig.withAbsoluteEncoderConfig(
new AbsoluteEncoderConfig()
.withOffset(Rotation2d.fromRotations(-0.084717).unaryMinus())),
driveConfig);
driveConfig,
wheelCircumference);
}

/**
Expand All @@ -63,15 +72,16 @@ public static Translation2d createNorthWestModuleTranslation() {
* @return the north east swerve module.
*/
public static SwerveModuleIO createNorthEastModule(
MechanismConfig steerConfig, MechanismConfig driveConfig) {
MechanismConfig steerConfig, MechanismConfig driveConfig, double wheelCircumference) {
return createModule(
new CAN(16, "swerve"),
new CAN(18, "swerve"),
new CAN(30, "swerve"),
steerConfig.withAbsoluteEncoderConfig(
new AbsoluteEncoderConfig()
.withOffset(Rotation2d.fromRotations(0.196777).unaryMinus())),
driveConfig);
driveConfig,
wheelCircumference);
}

/**
Expand All @@ -89,15 +99,16 @@ public static Translation2d createNorthEastModuleTranslation() {
* @return the south east swerve module.
*/
public static SwerveModuleIO createSouthEastModule(
MechanismConfig steerConfig, MechanismConfig driveConfig) {
MechanismConfig steerConfig, MechanismConfig driveConfig, double wheelCircumference) {
return createModule(
new CAN(12, "swerve"),
new CAN(22, "swerve"),
new CAN(26, "swerve"),
steerConfig.withAbsoluteEncoderConfig(
new AbsoluteEncoderConfig()
.withOffset(Rotation2d.fromRotations(0.276611).unaryMinus())),
driveConfig);
driveConfig,
wheelCircumference);
}

/**
Expand All @@ -115,15 +126,16 @@ public static Translation2d createSouthEastModuleTranslation() {
* @return the south west swerve module.
*/
public static SwerveModuleIO createSouthWestModule(
MechanismConfig steerConfig, MechanismConfig driveConfig) {
MechanismConfig steerConfig, MechanismConfig driveConfig, double wheelCircumference) {
return createModule(
new CAN(10, "swerve"),
new CAN(20, "swerve"),
new CAN(28, "swerve"),
steerConfig.withAbsoluteEncoderConfig(
new AbsoluteEncoderConfig()
.withOffset(Rotation2d.fromRotations(0.223145).unaryMinus())),
driveConfig);
driveConfig,
wheelCircumference);
}

/**
Expand Down

0 comments on commit 66d88f5

Please sign in to comment.