Initial Swerve

.project

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <projectDescription>
-	<name>RoboLib</name>
+	<name>potential-octo-eureka-RoboLib</name>
 	<comment>Project RoboLib created by Buildship.</comment>
 	<projects>
 	</projects>

gradle.properties

@@ -0,0 +1,5 @@
+wpiVersion = 2025.3.1
+
+ctreVersion = 25.1.0
+
+advantageKitVersion = 4.0.0

lib/.classpath

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<classpath>
+	<classpathentry kind="src" output="bin/main" path="src/main/java">
+		<attributes>
+			<attribute name="gradle_scope" value="main"/>
+			<attribute name="gradle_used_by_scope" value="main,test"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="src" output="bin/test" path="src/test/java">
+		<attributes>
+			<attribute name="gradle_scope" value="test"/>
+			<attribute name="gradle_used_by_scope" value="test"/>
+			<attribute name="test" value="true"/>
+		</attributes>
+	</classpathentry>
+	<classpathentry kind="src" path="build/generated/sources/annotationProcessor/java/main"/>
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-17/"/>
+	<classpathentry kind="con" path="org.eclipse.buildship.core.gradleclasspathcontainer"/>
+	<classpathentry kind="output" path="bin/default"/>
+</classpath>

lib/.project

@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>lib</name>
+	<comment>Project lib created by Buildship.</comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.buildship.core.gradleprojectbuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+		<nature>org.eclipse.buildship.core.gradleprojectnature</nature>
+	</natures>
+	<filteredResources>
+		<filter>
+			<id>1743377880685</id>
+			<name></name>
+			<type>30</type>
+			<matcher>
+				<id>org.eclipse.core.resources.regexFilterMatcher</id>
+				<arguments>node_modules|\.git|__CREATED_BY_JAVA_LANGUAGE_SERVER__</arguments>
+			</matcher>
+		</filter>
+	</filteredResources>
+</projectDescription>

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') {

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<modules.length;i++){
+            moduleStates[i].optimize(Rotation2d.fromRotations(modules[i].getCurrentSteerRotations()));
+        }
+
+        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);
+        }
+    }
+
+    public void setChassisSpeed(ChassisSpeeds chassisSpeeds){
+        this.chassisSpeeds = chassisSpeeds;
+    }
+
+    public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
+        this.chassisSpeeds = chassisSpeeds;
+        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()));
     }
 
 }

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

@@ -0,0 +1,103 @@
+package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.sun.jdi.request.StepRequest;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+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 TalonFX driveMotor;
+    private VelocityVoltage driveRequest = new VelocityVoltage(0);
+    private TalonFX steerMotor;
+    private PositionVoltage steerRequest = new PositionVoltage(0);
+    private CANcoder steerEncoder;
+
+    private Translation2d moduleLocation;
+
+    public SwerveModule(
+        int driveMotorId,
+        double driveKP,
+        double driveKI,
+        double driveKD,
+        int steerMotorId,
+        double steerKP,
+        double steerKI,
+        double steerKD,
+        int steerEncoderId,
+        String canBusName,
+        Translation2d moduleLocation,
+        double gearRatio,
+        double wheelDiameterInMeters
+    ){
+        driveMotor = new TalonFX(driveMotorId, canBusName);
+        driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
+        driveMotor.optimizeBusUtilization();
+        driveMotor.getConfigurator().apply(generateTalonConfiguration(driveKP, driveKI, driveKD));
+
+        steerMotor = new TalonFX(steerMotorId, canBusName);
+        steerMotor.optimizeBusUtilization();
+        steerMotor.getConfigurator().apply(generateTalonConfiguration(steerKP, steerKI, steerKD));
+
+        steerEncoder = new CANcoder(steerEncoderId, canBusName);
+        steerEncoder.getPosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
+        steerEncoder.optimizeBusUtilization();
+        steerEncoder.getConfigurator().apply(generateCanCoderConfiguration());
+
+        this.moduleLocation = moduleLocation;
+
+        this.gearRatio = gearRatio;
+        this.wheelDiameterInMeters = wheelDiameterInMeters;
+    }
+
+    public void setSteerRotations(double rotations){
+        steerMotor.setControl(steerRequest.withSlot(0).withPosition(rotations));
+    }
+
+    public double getCurrentSteerRotations(){
+        return steerEncoder.getPosition().refresh().getValueAsDouble();
+    }
+
+    public void setDriveSpeed(double speed){
+        double rotorVelocity = ((Math.PI * wheelDiameterInMeters) / speed) * gearRatio * TALON_BUILT_IN_ENCODER_UNITS_PER_ROTATION;
+        driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
+    }
+
+    public double getDriveSpeed(){
+        return driveMotor.getVelocity().refresh().getValueAsDouble();
+    }
+
+    public Translation2d getModuleLocation(){
+        return moduleLocation;
+    }
+
+    public SwerveModuleState getModuleState(){
+        return moduleState;
+    }
+
+    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();
+    }
+
+}