From 5270012b7458e29dccd47e26497e916954fb9bcf Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Mon, 17 Nov 2025 20:29:00 -0500 Subject: [PATCH] Swerve Builder --- .../SwerveDrive/SwerveBuilder.java | 124 ++++++++++++++++++ .../SwerveDrive/SwerveDriveSubsystem.java | 51 +++++-- .../SwerveDrive/SwerveModule.java | 28 +++- 3 files changed, 187 insertions(+), 16 deletions(-) create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java new file mode 100644 index 0000000..96a80f9 --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java @@ -0,0 +1,124 @@ +package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; + +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveBuilder { + + ArrayList modules = new ArrayList(); + + private int pigeonId; + + public SwerveBuilder(){ + + } + + public SwerveBuilder withPigeonId(int pigeonId){ + this.pigeonId = pigeonId; + return this; + } + + public SwerveBuilder withSwerveModule(SwerveModule module){ + this.modules.add(module); + return this; + } + + public SwerveDriveSubsystem build(){ + return new SwerveDriveSubsystem(pigeonId, modules.toArray(size -> new SwerveModule[size])); + } + + public class SwerveModuleBuilder { + + private int driveMotorId; + private double driveKP; + private double driveKI; + private double driveKD; + private int steerMotorId; + private double steerKP; + private double steerKI; + private double steerKD; + private int steerEncoderId; + private double steerOffset; + private String canBusName; + private Translation2d moduleLocation; + private double gearRatio; + private double wheelDiameterInMeters; + + public SwerveModuleBuilder() { + + } + + public SwerveModuleBuilder withDriveMotorId(int driveMotorId){ + this.driveMotorId = driveMotorId; + return this; + } + + public SwerveModuleBuilder withDriveKP(double driveKP){ + this.driveKP = driveKP; + return this; + } + + public SwerveModuleBuilder withDriveKI(double driveKI){ + this.driveKI = driveKI; + return this; + } + + public SwerveModuleBuilder withDriveKD(double driveKD){ + this.driveKD = driveKD; + return this; + } + + public SwerveModuleBuilder withSteerMotorId(int steerMotorId){ + this.steerMotorId = steerMotorId; + return this; + } + + public SwerveModuleBuilder withSteerKP(double steerKP){ + this.steerKP = steerKP; + return this; + } + + public SwerveModuleBuilder withSteerKI(double steerKI){ + this.steerKI = steerKI; + return this; + } + + public SwerveModuleBuilder withSteerKD(double steerKD){ + this.steerKD = steerKD; + return this; + } + + public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){ + this.steerEncoderId = steerEncoderId; + return this; + } + + public SwerveModuleBuilder withCanBusName(String canBusName){ + this.canBusName = canBusName; + return this; + } + + public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation){ + this.moduleLocation = moduleLocation; + return this; + } + + public SwerveModuleBuilder withGearRatio(double gearRatio){ + this.gearRatio = gearRatio; + return this; + } + + public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters){ + this.wheelDiameterInMeters = wheelDiameterInMeters; + return this; + } + + public SwerveModule build() { + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, steerMotorId, steerKP, steerKI, steerKD, + steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters); + } + + } + +} diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java index 34ac849..ce6eb7f 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java @@ -1,14 +1,21 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +import java.util.Arrays; + import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SwerveDriveSubsystem extends SubsystemBase{ @@ -17,6 +24,8 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private ChassisSpeeds chassisSpeeds; private Translation2d centerOfRotation; + private SwerveDrivePoseEstimator3d poseEstimator; + private SwerveModule[] modules; private final double MAX_LINEAR_SPEED = 3.9; @@ -24,15 +33,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private Pigeon2 gyro; - public SwerveDriveSubsystem(){ + public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){ + + this.modules = modules; driveKiniematics = new SwerveDriveKinematics( - modules[0].getModuleLocation(), - modules[1].getModuleLocation(), - modules[2].getModuleLocation(), - modules[3].getModuleLocation() + Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size]) ); + gyro = new Pigeon2(pigeonId); + + poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero); } @Override @@ -41,19 +52,28 @@ public class SwerveDriveSubsystem extends SubsystemBase{ Logger.recordOutput("Un-Optimized Swerve States", moduleStates); for(int i=0; i m.getModuleLocation()).toArray(Translation2d[]::new); + } + + public Angle getCurrentYaw(){ + return gyro.getYaw().refresh().getValue(); + } + public void setChassisSpeed(ChassisSpeeds chassisSpeeds){ - this.chassisSpeeds = chassisSpeeds; + this.setChassisSpeed(chassisSpeeds, Translation2d.kZero); } public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){ @@ -61,8 +81,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{ this.centerOfRotation = centerOfRotation; } - public void chassisSpeedsFromJoystick(double x, double y, double rot){ - ChassisSpeeds.fromFieldRelativeSpeeds(MAX_LINEAR_SPEED * x, MAX_LINEAR_SPEED * y, MAX_ANGULAR_SPEED * rot, Rotation2d.fromDegrees(gyro.getYaw().refresh().getValueAsDouble())); + public void setChassisSpeedsFromJoystick(double x, double y, double rot){ + this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds( + MAX_LINEAR_SPEED * x, + MAX_LINEAR_SPEED * y, + MAX_ANGULAR_SPEED * rot, + Rotation2d.fromDegrees(getCurrentYaw().magnitude()) + )); + } + + private SwerveModulePosition[] getModulePositions(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new); } } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java index b2b24db..45266db 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java @@ -1,6 +1,7 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; @@ -9,7 +10,9 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.sun.jdi.request.StepRequest; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; public class SwerveModule { @@ -42,6 +45,7 @@ public class SwerveModule { double steerKI, double steerKD, int steerEncoderId, + double steerOffset, String canBusName, Translation2d moduleLocation, double gearRatio, @@ -59,7 +63,7 @@ public class SwerveModule { steerEncoder = new CANcoder(steerEncoderId, canBusName); steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); steerEncoder.optimizeBusUtilization(); - steerEncoder.getConfigurator().apply(generateCanCoderConfiguration()); + steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset)); this.moduleLocation = moduleLocation; @@ -71,8 +75,8 @@ public class SwerveModule { steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations)); } - public double getCurrentSteerRotations(){ - return steerEncoder.getPosition().refresh().getValueAsDouble(); + public Rotation2d getCurrentSteerRotations(){ + return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble()); } public void setDriveSpeed(double speed){ @@ -88,16 +92,30 @@ public class SwerveModule { return moduleLocation; } + public void setModuleState(SwerveModuleState state){ + this.setSteerRotations(state.angle.getRotations()); + this.setDriveSpeed(state.speedMetersPerSecond); + } + public SwerveModuleState getModuleState(){ return moduleState; } + public SwerveModulePosition getModulePosition(){ + return new SwerveModulePosition(this.getModuleDistance(), getCurrentSteerRotations()); + } + + private double getModuleDistance(){ + double distance = (this.driveMotor.getPosition(true).getValueAsDouble() / TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION) / gearRatio / (Math.PI * wheelDiameterInMeters); + return distance; + } + private TalonFXConfiguration generateTalonConfiguration(double kP, double kI, double kD){ return new TalonFXConfiguration().withSlot0(new Slot0Configs().withKP(kP).withKI(kI).withKD(kD)); } - private CANcoderConfiguration generateCanCoderConfiguration(){ - return new CANcoderConfiguration(); + private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){ + return new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(steerOffset)); } }