FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Swerve Builder

This commit is contained in:
Tyler Wilcox 2025-11-17 20:29:00 -05:00
parent aa40781105
commit 5270012b74
3 changed files with 187 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveBuilder.java Normal file
View File

@ -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<SwerveModule> modules = new ArrayList<SwerveModule>();
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);
}
}
}

51
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java
View File

@ -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<modules.length;i++){
moduleStates[i].optimize(Rotation2d.fromRotations(modules[i].getCurrentSteerRotations()));
moduleStates[i].optimize(modules[i].getCurrentSteerRotations());
}
poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
Logger.recordOutput("Optimized Swerve States", moduleStates);
for(int i=0;i<modules.length;i++){
modules[i].setSteerRotations(moduleStates[i].angle.getRotations());
modules[i].setDriveSpeed(moduleStates[i].speedMetersPerSecond);
modules[i].setModuleState(moduleStates[i]);
}
}
public Translation2d[] getModuleLocations(){
return Arrays.stream(modules).map((SwerveModule m) -> 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);
}
}

28
lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveModule.java
View File

@ -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));
}
}