[Swerve, YAGSL] Configuration swerve subsystem

src/main/deploy/swerve/controllerproperties.json

@@ -0,0 +1,8 @@
+{
+  "angleJoystickRadiusDeadband": 0.5,
+  "heading": {
+    "p": 0.4,
+    "i": 0,
+    "d": 0.01
+  }
+}

src/main/deploy/swerve/modules/backleft.json

@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": 0,
+    "left": 0
+  },
+  "absoluteEncoderOffset": 0,
+  "drive": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "none",
+    "id": 0,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  },
+  "absoluteEncoderInverted": false
+}

src/main/deploy/swerve/modules/backright.json

@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": 0,
+    "left": 0
+  },
+  "absoluteEncoderOffset": 0,
+  "drive": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "none",
+    "id": 0,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  },
+  "absoluteEncoderInverted": false
+}

src/main/deploy/swerve/modules/frontleft.json

@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": 0,
+    "left": 0
+  },
+  "absoluteEncoderOffset": 0,
+  "drive": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "none",
+    "id": 0,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  },
+  "absoluteEncoderInverted": false
+}

src/main/deploy/swerve/modules/frontright.json

@@ -0,0 +1,27 @@
+{
+  "location": {
+    "front": 0,
+    "left": 0
+  },
+  "absoluteEncoderOffset": 0,
+  "drive": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkflex",
+    "id": 0,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "none",
+    "id": 0,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  },
+  "absoluteEncoderInverted": false
+}

src/main/deploy/swerve/modules/physicalproperties.json

@@ -0,0 +1,16 @@
+{
+  "optimalVoltage": 12,
+  "wheelGripCoefficientOfFriction": 1.19,
+  "currentLimit": {
+    "drive": 40,
+    "angle": 20
+  },
+  "conversionFactor": {
+    "angle": 0,
+    "drive": 0
+  },
+  "rampRate": {
+    "drive": 0.25,
+    "angle": 0.25
+  }
+}

src/main/deploy/swerve/modules/pidfproperties.json

@@ -0,0 +1,16 @@
+{
+  "drive": {
+    "p": 0.0020645,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  },
+  "angle": {
+    "p": 0.0020645,
+    "i": 0,
+    "d": 0,
+    "f": 0,
+    "iz": 0
+  }
+}

src/main/deploy/swerve/swervedrive.json

@@ -0,0 +1,14 @@
+{
+  "imu": {
+    "type": "pigeon2",
+    "id": 0,
+    "canbus": null
+  },
+  "invertedIMU": false,
+  "modules": [
+    "frontleft.json",
+    "frontright.json",
+    "backleft.json",
+    "backright.json"
+  ]
+}

src/main/java/frc/robot/Constants.java

@@ -1,7 +1,29 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
+
+import com.pathplanner.lib.util.PIDConstants;
+
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+
 public class Constants {
     public static class SwerveK {
-
+        public static final int driveMotorID = 0;
+        public static final int turnMotorID = 1;
+        public static final int encoderID = 2;
+
+        public static final Measure<Distance> wheelDiameter = Inches.of(3); 
+        public static final Measure<Distance> driveBaseRadius = Meters.of(0.4579874);
+
+        public static final double steerGearRatio = 12.8;
+        public static final double driveGearRatio = 6;
+
+        public static final double maxModuleSpeed = 0; //! Tune
+
+        public static final PIDConstants translationConstants = new PIDConstants(1, 1, 1); //! Tune
+        public static final PIDConstants rotationConstants = new PIDConstants(1, 1, 1); //! Tune
+
     }
 }

src/main/java/frc/robot/subsystems/Swerve.java

@@ -1,5 +1,109 @@
 package frc.robot.subsystems;
 
-public class Swerve {
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
 
+import java.io.File;
+import java.io.IOException;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.SwerveK;
+import swervelib.SwerveDrive;
+import swervelib.math.SwerveMath;
+import swervelib.parser.SwerveParser;
+import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
+
+public class Swerve extends SubsystemBase {
+
+    SwerveDrive swerveDrive;
+    double maximumSpeed = Units.feetToMeters(4.5);
+
+    /**
+     * 
+     * @param directory Directory of swerve drive config files.
+     */
+    public Swerve(File directory) throws IOException {
+        double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(SwerveK.steerGearRatio);
+        double driveConversionFactor = SwerveMath.calculateMetersPerRotation(SwerveK.driveGearRatio, SwerveK.wheelDiameter.in(Inches));
+
+        SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+
+        swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor);
+    }
+
+    public void setupPathPlanner() {
+        AutoBuilder.configureHolonomic(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getRobotVelocity, 
+            this::setChassisSpeeds, 
+            new HolonomicPathFollowerConfig(SwerveK.translationConstants, 
+                                            SwerveK.rotationConstants, 
+                                            SwerveK.maxModuleSpeed, 
+                                            SwerveK.driveBaseRadius.in(Meters), 
+                                            null), 
+            null,
+            this);
+    }
+
+    /**
+     * 
+     * @param TranslationX Translation in the X direction
+     * @param TranslationY Translation in the Y direction
+     * @param angularVelocity Angular Velocity to set 
+     * @return
+     */
+    public Command driveCommand(double TranslationX, double TranslationY, double angularVelocity) {
+        return runOnce(() -> drive(
+                new Translation2d(TranslationX * swerveDrive.getMaximumVelocity(), TranslationY * swerveDrive.getMaximumVelocity()), 
+                angularVelocity * swerveDrive.getMaximumAngularVelocity(), 
+                true, false));
+    }
+
+    public Command turnCommand(Measure<Angle> targetAngle, Measure<Angle> currentAngle, boolean fieldRelative) {
+        return runOnce(() -> {turn(targetAngle, currentAngle, fieldRelative);
+        });
+    }
+
+    public void turn(Measure<Angle> targetAngle, Measure<Angle> currentAngle, boolean fieldRelative) {
+        drive(getPose().getTranslation(), getTurningAngle(targetAngle, currentAngle).in(Degrees), fieldRelative, false);
+    }
+
+    public Measure<Angle> getTurningAngle(Measure<Angle> desiredAngle, Measure<Angle> currentHeading) {
+        double angle = (desiredAngle.minus(currentHeading).plus(Degrees.of(540))).in(Degrees);
+        angle = (angle % 360) - 180;
+        return Degrees.of(angle);
+    }
+
+    public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) {
+        swerveDrive.drive(translation, rotation, fieldRelative, isOpenLoop);
+    }
+
+    public void resetOdometry(Pose2d initialHolonomicPose) {
+        swerveDrive.resetOdometry(initialHolonomicPose);
+    }
+
+    public Pose2d getPose() {
+        return swerveDrive.getPose();
+    }
+
+    public ChassisSpeeds getRobotVelocity() {
+        return swerveDrive.getRobotVelocity();
+    }
+
+    public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+        swerveDrive.setChassisSpeeds(chassisSpeeds);
+    }
 }