From ea289de47bea948fbfff54cdb328bf1675f99097 Mon Sep 17 00:00:00 2001
From: atlanticbomber <atlanticbomber@gmail.com>
Date: Fri, 1 Nov 2024 19:12:13 -0700
Subject: [PATCH] added pathPlanner compatablity

---
 .../java/com/team2813/subsystems/Drive.java   | 56 +++++++++++++++++--
 SharpShooter/vendordeps/PathplannerLib.json   | 12 ++--
 2 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/SharpShooter/src/main/java/com/team2813/subsystems/Drive.java b/SharpShooter/src/main/java/com/team2813/subsystems/Drive.java
index 817e948..80ed0b0 100644
--- a/SharpShooter/src/main/java/com/team2813/subsystems/Drive.java
+++ b/SharpShooter/src/main/java/com/team2813/subsystems/Drive.java
@@ -24,14 +24,23 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.pathplanner.lib.auto.AutoBuilder;
+
 
 public class Drive extends SubsystemBase {
 
@@ -43,7 +52,15 @@ public class Drive extends SubsystemBase {
           .withSteerRequestType(SteerRequestType.MotionMagicExpo);
   SwerveDrivetrain drivetrain;
   private double multiplier = 1;
+  private static class PublicisizedKinematics extends SwerveDrivetrain {
+    public PublicisizedKinematics(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
+      super(driveTrainConstants, modules);
+    }
 
+    public ChassisSpeeds getChassisSpeeds() {
+      return m_kinematics.toChassisSpeeds(getState().ModuleStates);
+    }
+  }
   public Drive() {
 
     double FLSteerOffset = 0.4908046875;
@@ -113,9 +130,22 @@ public Drive() {
             true);
     SwerveModuleConstants[] constants =
         new SwerveModuleConstants[] {frontLeft, frontRight, backLeft, backRight};
-    SwerveDrivetrain drivetrain = new SwerveDrivetrain(drivetrainConstants, constants);
+    PublicisizedKinematics drivetrain = new PublicisizedKinematics(drivetrainConstants, constants);
     this.drivetrain = drivetrain;
-    
+    RobotConfig config = new RobotConfig(49.07, 5.26, new ModuleConfig(0.044, MAX_VELOCITY, 1.0,
+            DCMotor.getKrakenX60(1).withReduction(6.75),40.0, 1), 0.85, 0.939);
+    AutoBuilder.configure(
+            this::getAutoPose,
+            this::resetOdometry,
+            drivetrain::getChassisSpeeds,
+            this::drive,
+            new PPHolonomicDriveController(
+                    new PIDConstants(0, 0, 0), // Translation PID
+                    new PIDConstants(0, 0, 0) // Rotation PID
+            ),
+            config,
+            Drive::onRed,
+            this);
     for (int i = 0; i < 4; i++) {
       int temp = i;
       Shuffleboard.getTab("swerve").addDouble(String.format("Module [%d] position", i), () -> getPosition(temp));
@@ -125,15 +155,30 @@ public Drive() {
   private double getPosition(int moduleId) {
     return drivetrain.getModule(moduleId).getCANcoder().getAbsolutePosition().getValueAsDouble();
   }
+  private static boolean onRed() {
+    return DriverStation.getAlliance()
+            .<Boolean>map((j) -> j == DriverStation.Alliance.Red)
+            .orElse(false);
+  }
+  public Pose2d getAutoPose() {
+      return drivetrain.getState().Pose;
+  }
+  public void resetOdometry(Pose2d currentPose) {
+    drivetrain.seedFieldRelative(currentPose);
+  }
 
   public void stop() {
     drivetrain.setControl(new SwerveRequest.Idle());
   }
-
+  private final SwerveRequest.ApplyChassisSpeeds chassisSpeedsRequest =
+          new SwerveRequest.ApplyChassisSpeeds().withDriveRequestType(DriveRequestType.OpenLoopVoltage)
+                  .withSteerRequestType(SteerRequestType.MotionMagicExpo);
   public void enableSlowMode(boolean enable) {
     multiplier = enable ? 0.4 : 1;
   }
-
+  public void drive(ChassisSpeeds demand) {
+    drivetrain.setControl(chassisSpeedsRequest.withSpeeds(demand));
+  }
   public void drive(double x, double y, double rotation) {
     DriverStation.reportWarning(String.format("x: %f, y: %f, Ø: %f", x, y, rotation), false);
     drivetrain.setControl(
@@ -142,6 +187,7 @@ public void drive(double x, double y, double rotation) {
             .withVelocityY(y * multiplier)
             .withRotationalRate(rotation));
   }
+
   
   public Rotation2d getRotation() {
     return drivetrain.getRotation3d().toRotation2d();
@@ -160,4 +206,4 @@ public void periodic() {
     actualState.set(drivetrain.getState().ModuleStates);
     rotation.set(getRotation());
   }
-}
+}
\ No newline at end of file
diff --git a/SharpShooter/vendordeps/PathplannerLib.json b/SharpShooter/vendordeps/PathplannerLib.json
index 6dc648d..abf8202 100644
--- a/SharpShooter/vendordeps/PathplannerLib.json
+++ b/SharpShooter/vendordeps/PathplannerLib.json
@@ -1,18 +1,18 @@
 {
-    "fileName": "PathplannerLib.json",
+    "fileName": "PathplannerLib-beta.json",
     "name": "PathplannerLib",
-    "version": "2024.2.8",
+    "version": "2025.0.0-beta-3.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
         "https://3015rangerrobotics.github.io/pathplannerlib/repo"
     ],
-    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json",
     "javaDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.2.8"
+            "version": "2025.0.0-beta-3.1"
         }
     ],
     "jniDependencies": [],
@@ -20,15 +20,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.2.8",
+            "version": "2025.0.0-beta-3.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "linuxx86-64",
                 "osxuniversal",
+                "linuxx86-64",
                 "linuxathena",
                 "linuxarm32",
                 "linuxarm64"