Actually make pathplanner work

src/main/deploy/pathplanner/paths/New Path.path

@@ -3,29 +3,45 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 13.882261436839258,
+        "x": 13.16336824764045,
-        "y": 3.061093579814278
+        "y": 1.4493814298363055
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.693915037547589,
+        "x": 14.079377311296996,
-        "y": 2.6668618308988026
+        "y": 1.379811121204163
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.728700191862979,
+        "x": 13.16336824764045,
-        "y": 1.2522655553831117
+        "y": 2.6204816251440413
       },
       "prevControl": {
-        "x": 13.928641642594021,
+        "x": 14.160542671367828,
-        "y": 1.3218358640107113
+        "y": 2.550911316511899
+      },
+      "nextControl": {
+        "x": 12.223879630793721,
+        "y": 2.686027342598464
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 13.16336824764045,
+        "y": 1.4493814298363055
+      },
+      "prevControl": {
+        "x": 12.200978978229143,
+        "y": 1.518951738468449
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "mid"
     }
   ],
   "rotationTargets": [],

src/main/java/frc4388/robot/Constants.java

@@ -7,7 +7,15 @@
 
 package frc4388.robot;
 
-import com.pathplanner.lib.util.ReplanningConfig;
+import com.pathplanner.lib.config.ModuleConfig;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPLTVController;
+import com.pathplanner.lib.controllers.PathFollowingController;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+
+// import com.pathplanner.l;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc4388.utility.Gains;
@@ -91,7 +99,41 @@ public final class Constants {
             public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
             public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
 
-            public static final ReplanningConfig replanningConfig = new ReplanningConfig();
+            public static final double MASS = 10; // KG
+            public static final double MOI = 10;  // Moment of inertia
+
+            public static final double WHEEL_RADIUS = 0.05; // Meters
+            public static final double MAX_DRIVE_VELOCITY = 0.5; // Meters per second
+            public static final double MAX_ROT_SPEED = 0.2; // Rotations per second
+
+
+            public static final double COEFFICENT_OF_FRICTION = 1.0; // Between 0 and 1
+            public static final DCMotor TALON_SRX_MOTOR = DCMotor.getVex775Pro(1); //TODO: Get actual motor constants
+            public static final double DRIVE_CURRENT_LIMIT = 100000; //TODO: Get actual value
+
+
+            public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(
+                WHEEL_RADIUS, 
+                MAX_DRIVE_VELOCITY, 
+                COEFFICENT_OF_FRICTION, 
+                TALON_SRX_MOTOR,
+                DRIVE_CURRENT_LIMIT, 
+                2);
+ 
+            public static final RobotConfig PP_ROBOT_CONFIG = new RobotConfig(
+                MASS, 
+                MOI, 
+                MODULE_CONFIG,
+                new Translation2d[] {
+                    new Translation2d(),
+                    new Translation2d(),
+                    new Translation2d(),
+                    new Translation2d(),
+                });
+
+                
+            public static final double ROBOT_LOOP_TIME = 0.02; // Time it takes for the robot to run a loop
+            public static final PathFollowingController PP_PATH_FOLLOWING_CONTROLLER = new PPLTVController(ROBOT_LOOP_TIME);
         }
     
         public static final class Conversions {
@@ -135,7 +177,7 @@ public final class Constants {
       }
     
     public static final class VisionConstants {
-        public static final String CAMERA_NAME = "photonvision";
+        public static final String CAMERA_NAME = "Camera_Module_v1";
     }
 
     public static final class DriveConstants {

src/main/java/frc4388/robot/RobotContainer.java

@@ -14,6 +14,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.utility.controller.XboxController;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.robot.Constants.OIConstants;
@@ -133,6 +134,7 @@ public class RobotContainer {
 
 
         autoChooser = AutoBuilder.buildAutoChooser();
+        SmartDashboard.putData("Auto Chooser", autoChooser);
     }
 
     /**

src/main/java/frc4388/robot/subsystems/RobotLocalizer.java

@@ -4,6 +4,7 @@ import org.photonvision.PhotonCamera;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.DriveFeedforwards;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -11,6 +12,7 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.utility.RobotGyro;
@@ -29,17 +31,17 @@ public class RobotLocalizer extends SubsystemBase {
     @Override
     public void periodic() {
         // time
-        // Translation3d accel = gyro.getAcceleration();
+        Translation3d accel = gyro.getAcceleration3d();
 
-        // SmartDashboard.putNumber("Accel X", accel.getX());
+        SmartDashboard.putNumber("Accel X", accel.getX());
-        // SmartDashboard.putNumber("Accel Y", accel.getY());
+        SmartDashboard.putNumber("Accel Y", accel.getY());
-        // SmartDashboard.putNumber("Accel Z", accel.getZ());
+        SmartDashboard.putNumber("Accel Z", accel.getZ());
 
-        // Rotation3d rot = gyro.getRotation3d();
+        Rotation3d rot = gyro.getRotation3d();
 
-        // SmartDashboard.putNumber("Rot X", rot.getX());
+        SmartDashboard.putNumber("Rot X", rot.getX());
-        // SmartDashboard.putNumber("Rot Y", rot.getY());
+        SmartDashboard.putNumber("Rot Y", rot.getY());
-        // SmartDashboard.putNumber("Rot Z", rot.getZ());
+        SmartDashboard.putNumber("Rot Z", rot.getZ());
 
         // boolean tagExists = SmartDashboard.getBoolean("photonvision/Camera_Module_v1/hasTarget", false);
         
@@ -95,14 +97,10 @@ public class RobotLocalizer extends SubsystemBase {
 
     // PathPlanner
     public ChassisSpeeds getChassisSpeeds() {
-        return new ChassisSpeeds();
+        return new ChassisSpeeds(
+            0, 
+            0, 
+            Units.rotationsToRadians(gyro.getAngularVelocity())
+        );
     }
-    
-    // PathPlanner
-    public void setChassisSpeeds(ChassisSpeeds target) {
-        System.out.println(target);
-    }
-
-    
-
 }

src/main/java/frc4388/robot/subsystems/TankDrive.java

@@ -8,8 +8,11 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.controllers.PPLTVController;
 import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.DriveFeedforwards;
 
 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.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -32,18 +35,23 @@ public class TankDrive extends SubsystemBase{
 
         
         // Configure AutoBuilder last
-        AutoBuilder.configureLTV(
+        AutoBuilder.configure(
             robotLocalizer::getPose,
             robotLocalizer::resetPose,
             robotLocalizer::getChassisSpeeds,
-            robotLocalizer::setChassisSpeeds,
+            this::setTargetChassisSpeeds,
-            0.02, // Default loop time
+            AutoConstants.PP_PATH_FOLLOWING_CONTROLLER,
-            AutoConstants.replanningConfig,
+            AutoConstants.PP_ROBOT_CONFIG,
-            new BooleanSupplier() {
+            () -> {
-                @Override
+                // Boolean supplier that controls when the path will be mirrored for the red alliance
-                public boolean getAsBoolean() {
+                // This will flip the path being followed to the red side of the field.
-                    return false;
+                // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+    
+                var alliance = DriverStation.getAlliance();
+                if (alliance.isPresent()) {
+                    return alliance.get() == DriverStation.Alliance.Red;
                 }
+                return false;
             },
             this
         );
@@ -51,8 +59,8 @@ public class TankDrive extends SubsystemBase{
 
     private static final ControlMode mode = ControlMode.PercentOutput;
 
-    private static final double maxMoveSpeed = 0.6;
+    private static final double maxMoveSpeed = 0.3;
-    private static final double maxturnSpeed = 0.6;
+    private static final double maxturnSpeed = 0.3;
 
 
     private static final double[] turn = { //Right by default, motors are wired weirdly
@@ -71,7 +79,7 @@ public class TankDrive extends SubsystemBase{
 
     public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
 
-        double percent_move = (1. - Math.abs(rightStick.getX())) * leftStick.getY();
+        double percent_move = /* (1. - Math.abs(rightStick.getX())) */ leftStick.getY();
 
         double FR_rot = move[0] * percent_move + turn[0] * rightStick.getX();
         double FL_rot = move[1] * percent_move + turn[1] * rightStick.getX();
@@ -89,4 +97,19 @@ public class TankDrive extends SubsystemBase{
         SmartDashboard.putNumber("BR", BR_rot);
 
     }
+
+    private double clamp(double x, double min, double max){
+        return Math.max(Math.min(x, max), min);
+    }
+
+    private void setTargetChassisSpeeds(ChassisSpeeds target, DriveFeedforwards idk) {
+        double move = clamp(target.vxMetersPerSecond/AutoConstants.MAX_DRIVE_VELOCITY, -1, 1);
+        double rot = clamp(Units.radiansToRotations(target.omegaRadiansPerSecond)/AutoConstants.MAX_ROT_SPEED, -1, 1);
+
+        driveWithInput(
+            new Translation2d(0, move), 
+            new Translation2d(rot, 0),
+            false);
+
+    }
 }

src/main/java/frc4388/utility/RobotGyro.java

@@ -212,6 +212,10 @@ public class RobotGyro {
             m_pigeon.getAccelerationZ().getValue().baseUnitMagnitude());
     }
     
+    public double getAngularVelocity() {
+        return m_pigeon.getAngularVelocityYDevice().getValueAsDouble();
+    }
+
     public double getAngle() {
         // if (m_isGyroAPigeon) {
             return getPigeonAngles()[2];

vendordeps/PathplannerLib2024.json

@@ -1,18 +1,18 @@
 {
-    "fileName": "PathplannerLib.json",
+    "fileName": "PathplannerLib-beta.json",
     "name": "PathplannerLib",
-    "version": "2024.2.8",
+    "version": "2025.0.0-beta-5",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "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-5"
         }
     ],
     "jniDependencies": [],
@@ -20,15 +20,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.2.8",
+            "version": "2025.0.0-beta-5",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "linuxx86-64",
                 "osxuniversal",
+                "linuxx86-64",
                 "linuxathena",
                 "linuxarm32",
                 "linuxarm64"