From aa40781105747a446ccbb8a5c2c572d694c553cb Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Wed, 2 Apr 2025 19:04:13 -0400 Subject: [PATCH 01/15] Initial Swerve --- .project | 2 +- gradle.properties | 5 + lib/.classpath | 20 ++++ lib/.project | 34 ++++++ lib/build.gradle | 6 +- .../SwerveDrive/SwerveDriveSubsystem.java | 61 ++++++++++- .../SwerveDrive/SwerveModule.java | 103 ++++++++++++++++++ 7 files changed, 226 insertions(+), 5 deletions(-) create mode 100644 gradle.properties create mode 100644 lib/.classpath create mode 100644 lib/.project create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java diff --git a/.project b/.project index dc4fc5e..f6c755e 100644 --- a/.project +++ b/.project @@ -1,6 +1,6 @@ - RoboLib + potential-octo-eureka-RoboLib Project RoboLib created by Buildship. diff --git a/gradle.properties b/gradle.properties new file mode 100644 index 0000000..49abf93 --- /dev/null +++ b/gradle.properties @@ -0,0 +1,5 @@ +wpiVersion = 2025.3.1 + +ctreVersion = 25.1.0 + +advantageKitVersion = 4.0.0 diff --git a/lib/.classpath b/lib/.classpath new file mode 100644 index 0000000..a99ea3b --- /dev/null +++ b/lib/.classpath @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/lib/.project b/lib/.project new file mode 100644 index 0000000..cd81445 --- /dev/null +++ b/lib/.project @@ -0,0 +1,34 @@ + + + lib + Project lib created by Buildship. + + + + + org.eclipse.jdt.core.javabuilder + + + + + org.eclipse.buildship.core.gradleprojectbuilder + + + + + + org.eclipse.jdt.core.javanature + org.eclipse.buildship.core.gradleprojectnature + + + + 1743377880685 + + 30 + + org.eclipse.core.resources.regexFilterMatcher + node_modules|\.git|__CREATED_BY_JAVA_LANGUAGE_SERVER__ + + + + diff --git a/lib/build.gradle b/lib/build.gradle index c0e34dd..2618b96 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -25,9 +25,8 @@ repositories { mavenLocal() gradlePluginPortal() - maven {url "https://maven.photonvision.org/repository/snapshots"} - maven {url "https://maven.photonvision.org/repository/internal"} maven {url "https://maven.ctr-electronics.com/release/"} + maven {url "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"} } dependencies { @@ -48,6 +47,9 @@ dependencies { // CTRE Phoenix 6 implementation "com.ctre.phoenix6:wpiapi-java:${ctreVersion}" + // AdvantageKit + implementation "org.littletonrobotics.akit:akit-java:${advantageKitVersion}" + } tasks.named('test') { 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 243bf5f..34ac849 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,11 +1,68 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.geometry.Rotation2d; +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.SwerveModuleState; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SwerveDriveSubsystem extends SubsystemBase{ - + + private SwerveDriveKinematics driveKiniematics; + private ChassisSpeeds chassisSpeeds; + private Translation2d centerOfRotation; + + private SwerveModule[] modules; + + private final double MAX_LINEAR_SPEED = 3.9; + private final double MAX_ANGULAR_SPEED = Math.PI * 2; + + private Pigeon2 gyro; + public SwerveDriveSubsystem(){ - + + driveKiniematics = new SwerveDriveKinematics( + modules[0].getModuleLocation(), + modules[1].getModuleLocation(), + modules[2].getModuleLocation(), + modules[3].getModuleLocation() + ); + + } + + @Override + public void periodic() { + SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); + Logger.recordOutput("Un-Optimized Swerve States", moduleStates); + + for(int i=0; i Date: Mon, 17 Nov 2025 20:29:00 -0500 Subject: [PATCH 02/15] 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 void periodic() { 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 void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfR 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.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 SwerveModule( double steerKI, double steerKD, int steerEncoderId, + double steerOffset, String canBusName, Translation2d moduleLocation, double gearRatio, @@ -59,7 +63,7 @@ public 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 void setSteerRotations(double rotations){ 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 Translation2d getModuleLocation(){ 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)); } } From 9b7572f22efde6d047f010274e7ee3dbddcbb1db Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 6 Dec 2025 10:46:42 -0500 Subject: [PATCH 03/15] Improved Swerve Builder --- .../DriveSystems/SwerveDrive/SwerveBuilder.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 index 96a80f9..3953e9f 100644 --- 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 @@ -7,6 +7,7 @@ public class SwerveBuilder { ArrayList modules = new ArrayList(); + String canBusName = ""; private int pigeonId; @@ -19,6 +20,11 @@ public SwerveBuilder withPigeonId(int pigeonId){ return this; } + public SwerveBuilder withCanbusName(String canBusname){ + this.canBusName = canBusname; + return this; + } + public SwerveBuilder withSwerveModule(SwerveModule module){ this.modules.add(module); return this; @@ -40,7 +46,6 @@ public class SwerveModuleBuilder { private double steerKD; private int steerEncoderId; private double steerOffset; - private String canBusName; private Translation2d moduleLocation; private double gearRatio; private double wheelDiameterInMeters; @@ -94,11 +99,6 @@ public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){ return this; } - public SwerveModuleBuilder withCanBusName(String canBusName){ - this.canBusName = canBusName; - return this; - } - public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation){ this.moduleLocation = moduleLocation; return this; From 6f233e586fa8b5882c6346750677276c78c82ff7 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 12 Dec 2025 22:52:25 -0500 Subject: [PATCH 04/15] Swerve Builder updates --- lib/build.gradle | 3 ++- .../DriveSystems/SwerveDrive/SwerveBuilder.java | 10 ++++++++-- .../SwerveDrive/SwerveDriveSubsystem.java | 6 +----- .../DriveSystems/SwerveDrive/SwerveModule.java | 11 ++++++++--- .../org/fairportrobotics/frc/robolib/LibraryTest.java | 2 +- 5 files changed, 20 insertions(+), 12 deletions(-) diff --git a/lib/build.gradle b/lib/build.gradle index 2618b96..7fb47bc 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -31,7 +31,8 @@ repositories { dependencies { // Use JUnit Jupiter for testing. - testImplementation 'org.junit.jupiter:junit-jupiter:5.8.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' // WPI Lib implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion" 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 index 3953e9f..b47f429 100644 --- 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 @@ -7,7 +7,7 @@ public class SwerveBuilder { ArrayList modules = new ArrayList(); - String canBusName = ""; + String canBusName = "rio"; private int pigeonId; @@ -49,6 +49,7 @@ public class SwerveModuleBuilder { private Translation2d moduleLocation; private double gearRatio; private double wheelDiameterInMeters; + private String moduleName; public SwerveModuleBuilder() { @@ -114,9 +115,14 @@ public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters){ return this; } + public SwerveModuleBuilder withModuleName(String moduleName){ + this.moduleName = moduleName; + return this; + } + public SwerveModule build() { return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, steerMotorId, steerKP, steerKI, steerKD, - steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters); + steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); } } 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 ce6eb7f..82b7c97 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 @@ -2,8 +2,6 @@ import java.util.Arrays; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; @@ -44,12 +42,12 @@ public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){ gyro = new Pigeon2(pigeonId); poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero); + chassisSpeeds = new ChassisSpeeds(); } @Override public void periodic() { SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); - Logger.recordOutput("Un-Optimized Swerve States", moduleStates); for(int i=0; i Date: Sat, 24 Jan 2026 16:31:14 -0500 Subject: [PATCH 05/15] Not working... --- .../DriveSystems/SwerveDrive/SwerveDriveSubsystem.java | 4 ++++ .../frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) 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 82b7c97..aa7bbd5 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 @@ -64,6 +64,10 @@ public Translation2d[] getModuleLocations(){ return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleLocation()).toArray(Translation2d[]::new); } + public SwerveModuleState[] getModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new); + } + public Angle getCurrentYaw(){ return gyro.getYaw().refresh().getValue(); } 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 6a95ec6..3f9ec49 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 @@ -86,7 +86,7 @@ public Rotation2d getCurrentSteerRotations(){ public void setDriveSpeed(double speed){ double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio; - driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); + //driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); } public double getDriveSpeed(){ @@ -100,6 +100,7 @@ public Translation2d getModuleLocation(){ public void setModuleState(SwerveModuleState state){ this.setSteerRotations(state.angle.getRotations()); this.setDriveSpeed(state.speedMetersPerSecond); + moduleState = state; } public SwerveModuleState getModuleState(){ From bc53ef3b38fdbeee623ce0b188dbc7097b3cb405 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Mon, 26 Jan 2026 22:26:21 -0500 Subject: [PATCH 06/15] Hopeful fixes from the first round of tests --- .../SwerveDrive/SwerveBuilder.java | 25 ++++++- .../SwerveDrive/SwerveDriveSubsystem.java | 66 +++++++++++++++++-- .../SwerveDrive/SwerveModule.java | 57 +++++++++++++--- 3 files changed, 132 insertions(+), 16 deletions(-) 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 index b47f429..4b5781a 100644 --- 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 @@ -9,6 +9,9 @@ public class SwerveBuilder { ArrayList modules = new ArrayList(); String canBusName = "rio"; + private double maxAngularVelRadiansSecond = 1.0; + private double maxLinearVelMetersSecond = Math.PI; + private int pigeonId; public SwerveBuilder(){ @@ -30,8 +33,28 @@ public SwerveBuilder withSwerveModule(SwerveModule module){ return this; } + /** + * Set the max linear velocity in meters per second + * @param maxVel Max linear velocity in meters per second + * @return The swerve builder + */ + public SwerveBuilder withMaxLinearVelocity(double maxVel){ + this.maxLinearVelMetersSecond = maxVel; + return this; + } + + /** + * Set the max angular velocity in radians per second + * @param maxVel Max angular velocity in radians per second + * @return The swerve builder + */ + public SwerveBuilder withMaxAngularVelocity(double maxVel){ + this.maxAngularVelRadiansSecond = maxVel; + return this; + } + public SwerveDriveSubsystem build(){ - return new SwerveDriveSubsystem(pigeonId, modules.toArray(size -> new SwerveModule[size])); + return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); } public class SwerveModuleBuilder { 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 aa7bbd5..13e196d 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 @@ -2,6 +2,7 @@ import java.util.Arrays; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; @@ -26,12 +27,17 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private SwerveModule[] modules; - private final double MAX_LINEAR_SPEED = 3.9; - private final double MAX_ANGULAR_SPEED = Math.PI * 2; + private double MAX_LINEAR_SPEED = 3.9; + private double MAX_ANGULAR_SPEED = Math.PI * 2; private Pigeon2 gyro; - public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){ + private double mLastSimTime = 0; + + public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ + + MAX_LINEAR_SPEED = maxLinearVelMetersSecond; + MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond; this.modules = modules; @@ -47,19 +53,34 @@ public SwerveDriveSubsystem(int pigeonId, SwerveModule... modules){ @Override public void periodic() { + super.periodic(); SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); for(int i=0; i m.getModuleLocation()).toArray(Translation2d[]::new); } @@ -68,6 +89,14 @@ public SwerveModuleState[] getModuleStates(){ return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new); } + public SwerveModule[] getModules(){ + return Arrays.stream(modules).toArray(SwerveModule[]::new); + } + + public int getNumberOfModules(){ + return modules.length; + } + public Angle getCurrentYaw(){ return gyro.getYaw().refresh().getValue(); } @@ -81,7 +110,13 @@ public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfR this.centerOfRotation = centerOfRotation; } - public void setChassisSpeedsFromJoystick(double x, double y, double rot){ + /** + * Set robot speed relative to field + * @param x Forward robot speed in meters per second + * @param y Left robot speed in meters per second + * @param rot CCW robot speed in radians per second + */ + public void setChassisSpeedsFromJoystickFieldRelative(double x, double y, double rot){ this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds( MAX_LINEAR_SPEED * x, MAX_LINEAR_SPEED * y, @@ -90,6 +125,25 @@ public void setChassisSpeedsFromJoystick(double x, double y, double rot){ )); } + /** + * Set robot speed relative to robot + * @param x Forward robot speed in meters per second + * @param y Left robot speed in meters per second + * @param rot CCW robot speed in radians per second + */ + public void setChassisSpeedsFromJoystickRobotRelative(double x, double y, double rot){ + this.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds( + MAX_LINEAR_SPEED * x, + MAX_LINEAR_SPEED * y, + MAX_ANGULAR_SPEED * rot, + Rotation2d.fromDegrees(getCurrentYaw().magnitude()) + )); + } + + public Pose3d getRobotPose(){ + return poseEstimator.getEstimatedPosition(); + } + 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 3f9ec49..420b4c9 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,8 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -8,6 +10,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.sun.jdi.request.StepRequest; import edu.wpi.first.math.geometry.Rotation2d; @@ -57,11 +60,11 @@ public SwerveModule( driveMotor = new TalonFX(driveMotorId, canBusName); driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); driveMotor.optimizeBusUtilization(); - driveMotor.getConfigurator().apply(generateTalonConfiguration(driveKP, driveKI, driveKD)); + driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD)); steerMotor = new TalonFX(steerMotorId, canBusName); steerMotor.optimizeBusUtilization(); - steerMotor.getConfigurator().apply(generateTalonConfiguration(steerKP, steerKI, steerKD)); + steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD)); steerEncoder = new CANcoder(steerEncoderId, canBusName); steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); @@ -80,19 +83,33 @@ public void setSteerRotations(double rotations){ steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations)); } - public Rotation2d getCurrentSteerRotations(){ + public Rotation2d getSteerRotations(){ return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble()); } public void setDriveSpeed(double speed){ + // Convert wheel speed to rotor speed + // Circumfrence of wheel / speed * gear ratio double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio; - //driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); + driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); } - public double getDriveSpeed(){ + /** + * Get drive motor speed in rotations per second + * @return + */ + public double getDriveMotorSpeed(){ return driveMotor.getVelocity().refresh().getValueAsDouble(); } + /** + * Get drive wheel speed in meters per second + * @return + */ + public double getDriveWheelSpeed(){ + return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters); + } + public Translation2d getModuleLocation(){ return moduleLocation; } @@ -108,16 +125,38 @@ public SwerveModuleState getModuleState(){ } public SwerveModulePosition getModulePosition(){ - return new SwerveModulePosition(this.getModuleDistance(), getCurrentSteerRotations()); + return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations()); + } + + public String getModuleName(){ + return moduleName; } private double getModuleDistance(){ - double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / gearRatio / (Math.PI * wheelDiameterInMeters); + double distance = (this.driveMotor.getPosition(true).getValueAsDouble()) / 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 TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD){ + return new TalonFXConfiguration().withSlot0( + new Slot0Configs() + .withKP(kP) + .withKI(kI) + .withKD(kD) + ); + } + + private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD){ + return new TalonFXConfiguration().withSlot0( + new Slot0Configs() + .withKP(kP) + .withKI(kI) + .withKD(kD) + ).withFeedback( + new FeedbackConfigs() + .withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) + ); } private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){ From ae742134fec8c62eb2e8816f844068d11c8f018f Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Tue, 27 Jan 2026 20:46:13 -0500 Subject: [PATCH 07/15] Working logically --- .../SwerveDrive/SwerveBuilder.java | 16 +++++- .../SwerveDrive/SwerveDriveSubsystem.java | 10 ++-- .../SwerveDrive/SwerveModule.java | 51 ++++++++++++------- 3 files changed, 53 insertions(+), 24 deletions(-) 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 index 4b5781a..66ba603 100644 --- 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 @@ -54,7 +54,7 @@ public SwerveBuilder withMaxAngularVelocity(double maxVel){ } public SwerveDriveSubsystem build(){ - return new SwerveDriveSubsystem(pigeonId, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); + return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); } public class SwerveModuleBuilder { @@ -63,10 +63,12 @@ public class SwerveModuleBuilder { private double driveKP; private double driveKI; private double driveKD; + private double driveKV; private int steerMotorId; private double steerKP; private double steerKI; private double steerKD; + private double steerKV; private int steerEncoderId; private double steerOffset; private Translation2d moduleLocation; @@ -98,6 +100,11 @@ public SwerveModuleBuilder withDriveKD(double driveKD){ return this; } + public SwerveModuleBuilder withDriveKV(double driveKV){ + this.driveKV = driveKV; + return this; + } + public SwerveModuleBuilder withSteerMotorId(int steerMotorId){ this.steerMotorId = steerMotorId; return this; @@ -118,6 +125,11 @@ public SwerveModuleBuilder withSteerKD(double steerKD){ return this; } + public SwerveModuleBuilder withSteerKV(double steerKV){ + this.steerKV = steerKV; + return this; + } + public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){ this.steerEncoderId = steerEncoderId; return this; @@ -144,7 +156,7 @@ public SwerveModuleBuilder withModuleName(String moduleName){ } public SwerveModule build() { - return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, steerMotorId, steerKP, steerKI, steerKD, + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, steerKD, steerKV, steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); } 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 13e196d..5f65f70 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 @@ -34,7 +34,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private double mLastSimTime = 0; - public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ + public SwerveDriveSubsystem(int pigeonId, String canBusName, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ MAX_LINEAR_SPEED = maxLinearVelMetersSecond; MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond; @@ -45,7 +45,7 @@ public SwerveDriveSubsystem(int pigeonId, double maxLinearVelMetersSecond, doubl Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size]) ); - gyro = new Pigeon2(pigeonId); + gyro = new Pigeon2(pigeonId, canBusName); poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero); chassisSpeeds = new ChassisSpeeds(); @@ -56,9 +56,9 @@ public void periodic() { super.periodic(); SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); - for(int i=0; i Date: Tue, 27 Jan 2026 22:17:09 -0500 Subject: [PATCH 08/15] Hopeful fixes from the second round of testing --- .../SwerveDrive/SwerveBuilder.java | 67 ++++++++++++------- .../SwerveDrive/SwerveModule.java | 32 ++++++--- 2 files changed, 64 insertions(+), 35 deletions(-) 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 index 66ba603..0eed21a 100644 --- 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 @@ -14,47 +14,50 @@ public class SwerveBuilder { private int pigeonId; - public SwerveBuilder(){ + public SwerveBuilder() { } - public SwerveBuilder withPigeonId(int pigeonId){ + public SwerveBuilder withPigeonId(int pigeonId) { this.pigeonId = pigeonId; return this; } - public SwerveBuilder withCanbusName(String canBusname){ + public SwerveBuilder withCanbusName(String canBusname) { this.canBusName = canBusname; return this; } - public SwerveBuilder withSwerveModule(SwerveModule module){ + public SwerveBuilder withSwerveModule(SwerveModule module) { this.modules.add(module); return this; } /** * Set the max linear velocity in meters per second + * * @param maxVel Max linear velocity in meters per second * @return The swerve builder */ - public SwerveBuilder withMaxLinearVelocity(double maxVel){ + public SwerveBuilder withMaxLinearVelocity(double maxVel) { this.maxLinearVelMetersSecond = maxVel; return this; } /** * Set the max angular velocity in radians per second + * * @param maxVel Max angular velocity in radians per second * @return The swerve builder */ - public SwerveBuilder withMaxAngularVelocity(double maxVel){ + public SwerveBuilder withMaxAngularVelocity(double maxVel) { this.maxAngularVelRadiansSecond = maxVel; return this; } - public SwerveDriveSubsystem build(){ - return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); + public SwerveDriveSubsystem build() { + return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, + modules.toArray(size -> new SwerveModule[size])); } public class SwerveModuleBuilder { @@ -68,7 +71,9 @@ public class SwerveModuleBuilder { private double steerKP; private double steerKI; private double steerKD; + private double steerKS; private double steerKV; + private double steerKA; private int steerEncoderId; private double steerOffset; private Translation2d moduleLocation; @@ -80,84 +85,96 @@ public SwerveModuleBuilder() { } - public SwerveModuleBuilder withDriveMotorId(int driveMotorId){ + public SwerveModuleBuilder withDriveMotorId(int driveMotorId) { this.driveMotorId = driveMotorId; return this; } - public SwerveModuleBuilder withDriveKP(double driveKP){ + public SwerveModuleBuilder withDriveKP(double driveKP) { this.driveKP = driveKP; return this; } - public SwerveModuleBuilder withDriveKI(double driveKI){ + public SwerveModuleBuilder withDriveKI(double driveKI) { this.driveKI = driveKI; return this; } - public SwerveModuleBuilder withDriveKD(double driveKD){ + public SwerveModuleBuilder withDriveKD(double driveKD) { this.driveKD = driveKD; return this; } - public SwerveModuleBuilder withDriveKV(double driveKV){ + public SwerveModuleBuilder withDriveKV(double driveKV) { this.driveKV = driveKV; return this; } - public SwerveModuleBuilder withSteerMotorId(int steerMotorId){ + public SwerveModuleBuilder withSteerMotorId(int steerMotorId) { this.steerMotorId = steerMotorId; return this; } - public SwerveModuleBuilder withSteerKP(double steerKP){ + public SwerveModuleBuilder withSteerKP(double steerKP) { this.steerKP = steerKP; return this; } - public SwerveModuleBuilder withSteerKI(double steerKI){ + public SwerveModuleBuilder withSteerKI(double steerKI) { this.steerKI = steerKI; return this; } - public SwerveModuleBuilder withSteerKD(double steerKD){ + public SwerveModuleBuilder withSteerKD(double steerKD) { this.steerKD = steerKD; return this; } - public SwerveModuleBuilder withSteerKV(double steerKV){ + public SwerveModuleBuilder withSteerKS(double steerKS) { + this.steerKS = steerKS; + return this; + } + + public SwerveModuleBuilder withSteerKV(double steerKV) { this.steerKV = steerKV; return this; } - public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId){ + public SwerveModuleBuilder withSteerKA(double steerKA) { + this.steerKA = steerKA; + return this; + } + + public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) { this.steerEncoderId = steerEncoderId; return this; } - public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation){ + public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) { this.moduleLocation = moduleLocation; return this; } - public SwerveModuleBuilder withGearRatio(double gearRatio){ + public SwerveModuleBuilder withGearRatio(double gearRatio) { this.gearRatio = gearRatio; return this; } - public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters){ + public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters) { this.wheelDiameterInMeters = wheelDiameterInMeters; return this; } - public SwerveModuleBuilder withModuleName(String moduleName){ + public SwerveModuleBuilder withModuleName(String moduleName) { this.moduleName = moduleName; return this; } public SwerveModule build() { - return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, steerKD, steerKV, - steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, + steerKD, steerKS, steerKV, steerKA, + steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, + moduleName); } } 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 54a54de..f812215 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 @@ -4,8 +4,10 @@ import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; @@ -18,7 +20,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.units.measure.Angle; public class SwerveModule { @@ -35,7 +36,7 @@ public class SwerveModule { private TalonFX driveMotor; private VelocityVoltage driveRequest = new VelocityVoltage(0); private TalonFX steerMotor; - private PositionVoltage steerRequest = new PositionVoltage(0); + private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0); private CANcoder steerEncoder; private String moduleName; @@ -52,7 +53,9 @@ public SwerveModule( double steerKP, double steerKI, double steerKD, + double steerKS, double steerKV, + double steerKA, int steerEncoderId, double steerOffset, String canBusName, @@ -74,7 +77,7 @@ public SwerveModule( steerMotor = new TalonFX(steerMotorId, canBusName); steerMotor.optimizeBusUtilization(); - steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKV)); + steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); this.moduleLocation = moduleLocation; @@ -84,17 +87,19 @@ public SwerveModule( this.moduleName = moduleName; } - public void setSteerRotations(Angle rotations){ - steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations)); + public void setSteerRotations(Rotation2d rotations){ + steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getMeasure())); } public Rotation2d getSteerRotations(){ - return new Rotation2d(steerEncoder.getPosition().refresh().getValueAsDouble()); + // Maybe go back to non-Absolute Pos + // Or try reading from the steer motot for the pos + return new Rotation2d(steerEncoder.getAbsolutePosition().getValue()); } public void setDriveSpeed(double speed){ // Convert wheel speed to rotor speed - // Circumfrence of wheel / speed * gear ratio + // (speed / Circumfrence of wheel) * gear ratio double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio; driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); } @@ -120,7 +125,7 @@ public Translation2d getModuleLocation(){ } public void setModuleState(SwerveModuleState state){ - this.setSteerRotations(state.angle.getMeasure()); + this.setSteerRotations(state.angle); this.setDriveSpeed(state.speedMetersPerSecond); moduleState = state; } @@ -152,14 +157,21 @@ private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double k ); } - private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kV){ + private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){ return new TalonFXConfiguration().withSlot0( new Slot0Configs() .withKP(kP) .withKI(kI) .withKD(kD) - .withKS(kV) + .withKS(kS) + .withKV(kV) + .withKA(kA) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) + ).withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(80) + .withMotionMagicAcceleration(160) + .withMotionMagicJerk(1600) ).withFeedback( new FeedbackConfigs() .withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) From f9c7df64baa94652ae0f8f154962aac47069d8f0 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 29 Jan 2026 20:53:53 -0500 Subject: [PATCH 09/15] Working but offsets are busted --- .../SwerveDrive/SwerveBuilder.java | 13 +++- .../SwerveDrive/SwerveDriveSubsystem.java | 23 ++++--- .../SwerveDrive/SwerveModule.java | 68 +++++++++++++------ 3 files changed, 73 insertions(+), 31 deletions(-) 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 index 0eed21a..ca3eb28 100644 --- 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 @@ -67,6 +67,7 @@ public class SwerveModuleBuilder { private double driveKI; private double driveKD; private double driveKV; + private boolean driveInverted = false; private int steerMotorId; private double steerKP; private double steerKI; @@ -110,6 +111,11 @@ public SwerveModuleBuilder withDriveKV(double driveKV) { return this; } + public SwerveModuleBuilder withDriveInverted(){ + this.driveInverted = true; + return this; + } + public SwerveModuleBuilder withSteerMotorId(int steerMotorId) { this.steerMotorId = steerMotorId; return this; @@ -150,6 +156,11 @@ public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) { return this; } + public SwerveModuleBuilder withSteerOffset(double offsetRotations) { + this.steerOffset = offsetRotations; + return this; + } + public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) { this.moduleLocation = moduleLocation; return this; @@ -171,7 +182,7 @@ public SwerveModuleBuilder withModuleName(String moduleName) { } public SwerveModule build() { - return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, steerMotorId, steerKP, steerKI, + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, driveInverted, steerMotorId, steerKP, steerKI, steerKD, steerKS, steerKV, steerKA, steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); 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 5f65f70..7034431 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 @@ -54,16 +54,17 @@ public SwerveDriveSubsystem(int pigeonId, String canBusName, double maxLinearVel @Override public void periodic() { super.periodic(); - SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); + SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds); - // for(int i=0; i m.getModuleLocation()).toArray(Translation2d[]::new); } - public SwerveModuleState[] getModuleStates(){ - return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleState()).toArray(SwerveModuleState[]::new); + public SwerveModuleState[] getRequestedModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getRequestedModuleState()).toArray(SwerveModuleState[]::new); + } + + public SwerveModuleState[] getActualModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getActualModuleState()).toArray(SwerveModuleState[]::new); } public SwerveModule[] getModules(){ @@ -98,11 +103,11 @@ public int getNumberOfModules(){ } public Angle getCurrentYaw(){ - return gyro.getYaw().refresh().getValue(); + return gyro.getRoll().getValue(); } public void setChassisSpeed(ChassisSpeeds chassisSpeeds){ - this.setChassisSpeed(chassisSpeeds, Translation2d.kZero); + this.setChassisSpeed(chassisSpeeds, new Translation2d()); } public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){ 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 f812215..ad3f057 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,10 +1,13 @@ package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +import java.lang.reflect.Modifier; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -13,6 +16,8 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -20,23 +25,22 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SwerveModule { private final int CAN_UPDATE_FREQUENCY = 50; - private final int TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION = 2048; - private final int CANCODER_UNITS_PER_ROTATION = 2048; - private final double gearRatio; private final double wheelDiameterInMeters; - private SwerveModuleState moduleState; + private SwerveModuleState requestedModuleState; private TalonFX driveMotor; private VelocityVoltage driveRequest = new VelocityVoltage(0); private TalonFX steerMotor; private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0); + private PositionVoltage steerPosRequ = new PositionVoltage(0); private CANcoder steerEncoder; private String moduleName; @@ -49,6 +53,7 @@ public SwerveModule( double driveKI, double driveKD, double driveKV, + boolean driveInverted, int steerMotorId, double steerKP, double steerKI, @@ -66,18 +71,19 @@ public SwerveModule( ){ steerEncoder = new CANcoder(steerEncoderId, canBusName); - steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); - steerEncoder.optimizeBusUtilization(); + // steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); + // steerEncoder.optimizeBusUtilization(); steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset)); driveMotor = new TalonFX(driveMotorId, canBusName); - driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); - driveMotor.optimizeBusUtilization(); - driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV)); + // driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); + // driveMotor.optimizeBusUtilization(); + driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted)); steerMotor = new TalonFX(steerMotorId, canBusName); - steerMotor.optimizeBusUtilization(); + // steerMotor.optimizeBusUtilization(); steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); + // steerMotor.setNeutralMode(NeutralModeValue.Brake); this.moduleLocation = moduleLocation; @@ -88,7 +94,9 @@ public SwerveModule( } public void setSteerRotations(Rotation2d rotations){ - steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getMeasure())); + // steerMotor.setVoltage(steerPid.calculate(getSteerRotations().getRadians(), rotations.getRadians())); + // steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getRotations())); + steerMotor.setControl(steerRequest.withPosition(rotations.getRotations())); } public Rotation2d getSteerRotations(){ @@ -124,20 +132,33 @@ public Translation2d getModuleLocation(){ return moduleLocation; } - public void setModuleState(SwerveModuleState state){ + public void setRequestedModuleState(SwerveModuleState state){ this.setSteerRotations(state.angle); this.setDriveSpeed(state.speedMetersPerSecond); - moduleState = state; + requestedModuleState = state; } - public SwerveModuleState getModuleState(){ - return moduleState; + public SwerveModuleState getRequestedModuleState(){ + return requestedModuleState; + } + + public SwerveModuleState getActualModuleState() { + return new SwerveModuleState(getDriveWheelSpeed(), getSteerRotations()); } public SwerveModulePosition getModulePosition(){ return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations()); } + public void periodic(){ + SmartDashboard.putNumber(moduleName + " steerReq", steerPosRequ.Position); + SmartDashboard.putNumber(moduleName + " steerPow", steerMotor.getClosedLoopOutput().getValueAsDouble()); + SmartDashboard.putNumber(moduleName + " steer error", steerMotor.getClosedLoopError().getValueAsDouble()); + SmartDashboard.putNumber(moduleName + " steer setpoint", steerMotor.getClosedLoopReference().getValueAsDouble()); + + SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations()); + } + public String getModuleName(){ return moduleName; } @@ -147,14 +168,18 @@ private double getModuleDistance(){ return distance; } - private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV){ + private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV, boolean driveInverted){ return new TalonFXConfiguration().withSlot0( new Slot0Configs() .withKP(kP) .withKI(kI) .withKD(kD) .withKV(kV) - ); + ) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(driveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive) + ); } private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){ @@ -169,13 +194,13 @@ private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double k .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) ).withMotionMagic( new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(80) + .withMotionMagicCruiseVelocity(160) .withMotionMagicAcceleration(160) .withMotionMagicJerk(1600) ).withFeedback( new FeedbackConfigs() .withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) .withSensorToMechanismRatio(1.0) .withRotorToSensorRatio(12.8) ).withClosedLoopGeneral( @@ -187,8 +212,9 @@ private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double k private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){ return new CANcoderConfiguration().withMagnetSensor( new MagnetSensorConfigs() - .withMagnetOffset(steerOffset) - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(-steerOffset) + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(0.5) ); } From b269ce00bddf7b3435c254c4e7bf6a8b48067c60 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 29 Jan 2026 21:46:14 -0500 Subject: [PATCH 10/15] organizational changes --- .../SwerveDrive => drivesystems/swerve}/SwerveBuilder.java | 2 +- .../swerve}/SwerveDriveSubsystem.java | 2 +- .../SwerveDrive => drivesystems/swerve}/SwerveModule.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename lib/src/main/java/org/fairportrobotics/frc/robolib/{DriveSystems/SwerveDrive => drivesystems/swerve}/SwerveBuilder.java (98%) rename lib/src/main/java/org/fairportrobotics/frc/robolib/{DriveSystems/SwerveDrive => drivesystems/swerve}/SwerveDriveSubsystem.java (98%) rename lib/src/main/java/org/fairportrobotics/frc/robolib/{DriveSystems/SwerveDrive => drivesystems/swerve}/SwerveModule.java (99%) 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/swerve/SwerveBuilder.java similarity index 98% rename from lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java rename to lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java index ca3eb28..9e46b49 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java @@ -1,4 +1,4 @@ -package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +package org.fairportrobotics.frc.robolib.drivesystems.swerve; import java.util.ArrayList; 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/swerve/SwerveDriveSubsystem.java similarity index 98% rename from lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java rename to lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java index 7034431..53f98ac 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java @@ -1,4 +1,4 @@ -package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +package org.fairportrobotics.frc.robolib.drivesystems.swerve; import java.util.Arrays; 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/swerve/SwerveModule.java similarity index 99% rename from lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java rename to lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java index ad3f057..26a17ff 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java @@ -1,4 +1,4 @@ -package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; +package org.fairportrobotics.frc.robolib.drivesystems.swerve; import java.lang.reflect.Modifier; From 51cd3df642ff9d8ab5d2220884d6234ab517b4dd Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 29 Jan 2026 21:54:53 -0500 Subject: [PATCH 11/15] Fix warnings --- .../frc/robolib/drivesystems/swerve/SwerveBuilder.java | 10 ++++++---- .../drivesystems/swerve/SwerveDriveSubsystem.java | 5 +++-- .../frc/robolib/drivesystems/swerve/SwerveModule.java | 9 +++++---- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java index 9e46b49..ffbbf52 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java @@ -2,12 +2,14 @@ import java.util.ArrayList; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.math.geometry.Translation2d; public class SwerveBuilder { ArrayList modules = new ArrayList(); - String canBusName = "rio"; + CANBus canBusObj; private double maxAngularVelRadiansSecond = 1.0; private double maxLinearVelMetersSecond = Math.PI; @@ -24,7 +26,7 @@ public SwerveBuilder withPigeonId(int pigeonId) { } public SwerveBuilder withCanbusName(String canBusname) { - this.canBusName = canBusname; + this.canBusObj = new CANBus(canBusname); return this; } @@ -56,7 +58,7 @@ public SwerveBuilder withMaxAngularVelocity(double maxVel) { } public SwerveDriveSubsystem build() { - return new SwerveDriveSubsystem(pigeonId, canBusName, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, + return new SwerveDriveSubsystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); } @@ -184,7 +186,7 @@ public SwerveModuleBuilder withModuleName(String moduleName) { public SwerveModule build() { return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, driveInverted, steerMotorId, steerKP, steerKI, steerKD, steerKS, steerKV, steerKA, - steerEncoderId, steerOffset, canBusName, moduleLocation, gearRatio, wheelDiameterInMeters, + steerEncoderId, steerOffset, canBusObj, moduleLocation, gearRatio, wheelDiameterInMeters, moduleName); } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java index 53f98ac..1d928d0 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java @@ -2,6 +2,7 @@ import java.util.Arrays; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; @@ -34,7 +35,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private double mLastSimTime = 0; - public SwerveDriveSubsystem(int pigeonId, String canBusName, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ + public SwerveDriveSubsystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ MAX_LINEAR_SPEED = maxLinearVelMetersSecond; MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond; @@ -45,7 +46,7 @@ public SwerveDriveSubsystem(int pigeonId, String canBusName, double maxLinearVel Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size]) ); - gyro = new Pigeon2(pigeonId, canBusName); + gyro = new Pigeon2(pigeonId, canBus); poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero); chassisSpeeds = new ChassisSpeeds(); diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java index 26a17ff..9f1b818 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java @@ -2,6 +2,7 @@ import java.lang.reflect.Modifier; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -63,24 +64,24 @@ public SwerveModule( double steerKA, int steerEncoderId, double steerOffset, - String canBusName, + CANBus canBusObj, Translation2d moduleLocation, double gearRatio, double wheelDiameterInMeters, String moduleName ){ - steerEncoder = new CANcoder(steerEncoderId, canBusName); + steerEncoder = new CANcoder(steerEncoderId, canBusObj); // steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); // steerEncoder.optimizeBusUtilization(); steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset)); - driveMotor = new TalonFX(driveMotorId, canBusName); + driveMotor = new TalonFX(driveMotorId, canBusObj); // driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); // driveMotor.optimizeBusUtilization(); driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted)); - steerMotor = new TalonFX(steerMotorId, canBusName); + steerMotor = new TalonFX(steerMotorId, canBusObj); // steerMotor.optimizeBusUtilization(); steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); // steerMotor.setNeutralMode(NeutralModeValue.Brake); From 8defb590f40120ea22befb0e0880bcbb2636b502 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 30 Jan 2026 20:04:33 -0500 Subject: [PATCH 12/15] Possible Fix for swerve drive --- .../frc/robolib/drivesystems/swerve/SwerveModule.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java index 9f1b818..34d6d80 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java @@ -41,7 +41,6 @@ public class SwerveModule { private VelocityVoltage driveRequest = new VelocityVoltage(0); private TalonFX steerMotor; private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0); - private PositionVoltage steerPosRequ = new PositionVoltage(0); private CANcoder steerEncoder; private String moduleName; @@ -152,7 +151,6 @@ public SwerveModulePosition getModulePosition(){ } public void periodic(){ - SmartDashboard.putNumber(moduleName + " steerReq", steerPosRequ.Position); SmartDashboard.putNumber(moduleName + " steerPow", steerMotor.getClosedLoopOutput().getValueAsDouble()); SmartDashboard.putNumber(moduleName + " steer error", steerMotor.getClosedLoopError().getValueAsDouble()); SmartDashboard.putNumber(moduleName + " steer setpoint", steerMotor.getClosedLoopReference().getValueAsDouble()); @@ -207,6 +205,9 @@ private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double k ).withClosedLoopGeneral( new ClosedLoopGeneralConfigs() .withContinuousWrap(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) ); } @@ -214,7 +215,7 @@ private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){ return new CANcoderConfiguration().withMagnetSensor( new MagnetSensorConfigs() .withMagnetOffset(-steerOffset) - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(0.5) ); } From 817515901ca237b9f47e2001f7d65f9e38249eb1 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 30 Jan 2026 22:25:01 -0500 Subject: [PATCH 13/15] Reorganization --- .../robolib/drivesystems/swerve/SwerveBuilder.java | 4 ++-- ...eDriveSubsystem.java => SwerveDriveSystem.java} | 12 +++++++----- .../robolib/drivesystems/swerve/SwerveModule.java | 14 ++++++++++++++ 3 files changed, 23 insertions(+), 7 deletions(-) rename lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/{SwerveDriveSubsystem.java => SwerveDriveSystem.java} (94%) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java index ffbbf52..abdfc26 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java @@ -57,8 +57,8 @@ public SwerveBuilder withMaxAngularVelocity(double maxVel) { return this; } - public SwerveDriveSubsystem build() { - return new SwerveDriveSubsystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, + public SwerveDriveSystem build() { + return new SwerveDriveSystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, modules.toArray(size -> new SwerveModule[size])); } diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java similarity index 94% rename from lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java rename to lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java index 1d928d0..b23974b 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSubsystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java @@ -16,9 +16,10 @@ 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.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class SwerveDriveSubsystem extends SubsystemBase{ +public class SwerveDriveSystem implements Subsystem{ private SwerveDriveKinematics driveKiniematics; private ChassisSpeeds chassisSpeeds; @@ -35,7 +36,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{ private double mLastSimTime = 0; - public SwerveDriveSubsystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ + public SwerveDriveSystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ MAX_LINEAR_SPEED = maxLinearVelMetersSecond; MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond; @@ -54,7 +55,6 @@ public SwerveDriveSubsystem(int pigeonId, CANBus canBus, double maxLinearVelMete @Override public void periodic() { - super.periodic(); SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds); for(int i=0; i Date: Sat, 31 Jan 2026 14:03:26 -0500 Subject: [PATCH 14/15] It's working --- .../drivesystems/swerve/SwerveModule.java | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java index 43651d5..fb4375d 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; @@ -26,11 +27,14 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SwerveModule { private final int CAN_UPDATE_FREQUENCY = 50; + private final double STEER_MAX_CURRENT = 60; + private final double DRIVE_MAX_CURRENT = 80; private final double gearRatio; private final double wheelDiameterInMeters; @@ -41,7 +45,8 @@ public class SwerveModule { private VelocityVoltage driveRequest = new VelocityVoltage(0); private TalonFX steerMotor; - private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0); + private MotionMagicVoltage steerMMRequest = new MotionMagicVoltage(0); + private PositionVoltage steerPosRequest = new PositionVoltage(0); private CANcoder steerEncoder; @@ -96,9 +101,8 @@ public SwerveModule( } public void setSteerRotations(Rotation2d rotations){ - // steerMotor.setVoltage(steerPid.calculate(getSteerRotations().getRadians(), rotations.getRadians())); - // steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations.getRotations())); - steerMotor.setControl(steerRequest.withPosition(rotations.getRotations())); + steerMotor.setControl(steerPosRequest.withPosition(rotations.getRotations())); + // steerMotor.setControl(steerMMRequest.withPosition(rotations.getRotations())); } public Rotation2d getSteerRotations(){ @@ -127,7 +131,7 @@ public double getDriveMotorSpeed(){ * @return */ public double getDriveWheelSpeed(){ - return (driveMotor.getVelocity().refresh().getValueAsDouble() * gearRatio) / (Math.PI * wheelDiameterInMeters); + return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters); } public Translation2d getModuleLocation(){ @@ -177,7 +181,7 @@ public String getModuleName(){ } private double getModuleDistance(){ - double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) / (Math.PI * wheelDiameterInMeters); + double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) * (Math.PI * wheelDiameterInMeters); return distance; } @@ -192,6 +196,10 @@ private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double k .withMotorOutput( new MotorOutputConfigs() .withInverted(driveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive) + ).withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(DRIVE_MAX_CURRENT) ); } @@ -221,7 +229,11 @@ private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double k .withContinuousWrap(true) ).withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) + .withInverted(InvertedValue.Clockwise_Positive) + ).withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(STEER_MAX_CURRENT) ); } From 2845c4c95897607ea2a34293948051660df6baf4 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 31 Jan 2026 17:31:45 -0500 Subject: [PATCH 15/15] Documentation --- gradle.properties | 4 +-- lib/.project | 2 +- .../swerve/SwerveDriveSystem.java | 26 ++++++++++++------- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/gradle.properties b/gradle.properties index 49abf93..fa55bd4 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1,5 +1,5 @@ -wpiVersion = 2025.3.1 +wpiVersion = 2026.2.1 -ctreVersion = 25.1.0 +ctreVersion = 26.1.0 advantageKitVersion = 4.0.0 diff --git a/lib/.project b/lib/.project index cd81445..1ddb7d9 100644 --- a/lib/.project +++ b/lib/.project @@ -1,6 +1,6 @@ - lib + RoboLib-lib Project lib created by Buildship. diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java index b23974b..9a68f93 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java @@ -17,7 +17,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SwerveDriveSystem implements Subsystem{ @@ -55,7 +54,7 @@ public SwerveDriveSystem(int pigeonId, CANBus canBus, double maxLinearVelMetersS @Override public void periodic() { - SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds); + SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); for(int i=0; i