Swerve Builder

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

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(),
+            Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size])
-            modules[1].getModuleLocation(),
-            modules[2].getModuleLocation(),
-            modules[3].getModuleLocation()
         );
 
+        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].setModuleState(moduleStates[i]);
-            modules[i].setDriveSpeed(moduleStates[i].speedMetersPerSecond);
         }
     }
 
+    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){
+    public void setChassisSpeedsFromJoystick(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()));
+        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);
     }
 
 }

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(){
+    public Rotation2d getCurrentSteerRotations(){
-        return steerEncoder.getPosition().refresh().getValueAsDouble();
+        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(){
+    private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){
-        return new CANcoderConfiguration();
+        return new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(steerOffset));
     }
 
 }