From c47c6f8bff17f4426eab50843735c02653b7528c Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 7 Jan 2024 14:11:41 -0800
Subject: [PATCH 01/10] Implemented Pathplanner and suffixed all commands with
 Cmd

---
 src/main/deploy/pathplanner/navgrid.json      |  1 +
 .../subsystems/swerve/SwerveSubsystem.java    | 80 +++++++++++++------
 vendordeps/PathplannerLib.json                | 38 +++++++++
 3 files changed, 94 insertions(+), 25 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/navgrid.json
 create mode 100644 vendordeps/PathplannerLib.json

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 00000000..7026fb97
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"height":8.02},"nodeSizeMeters":0.2,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 76dd56e0..2bc11c8c 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -14,6 +14,11 @@
 package frc.robot.subsystems.swerve;
 
 import com.google.common.collect.Streams;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -30,6 +35,7 @@
 import java.util.Arrays;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -66,6 +72,21 @@ public class SwerveSubsystem extends SubsystemBase {
   public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
     this.gyroIO = gyroIO;
     modules = (Module[]) Arrays.stream(moduleIOs).map(Module::new).toArray();
+
+    AutoBuilder.configureHolonomic(
+                this::getPose, // Robot pose supplier
+                this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
+                this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                this::consumeChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
+                new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
+                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants
+                        4.5, // Max module speed, in m/s
+                        0.4, // Drive base radius in meters. Distance from robot center to furthest module.
+                        new ReplanningConfig() // Default path replanning config. See the API for the options here
+                ),
+                this // Reference to this subsystem to set requirements
+        );
   }
 
   /**
@@ -151,60 +172,63 @@ public void periodic() {
     }
   }
 
+  private void consumeChassisSpeeds(ChassisSpeeds speeds){
+    // Calculate module setpoints
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
+        SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
+
+        // Send setpoints to modules
+        SwerveModuleState[] optimizedSetpointStates =
+            Streams.zip(
+                    Arrays.stream(modules),
+                    Arrays.stream(setpointStates),
+                    (m, s) -> m.runSetpoint(s))
+                .toArray(SwerveModuleState[]::new);
+
+        // Log setpoint states
+        Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
+        Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
+  }
+
   /**
    * Runs the drive at the desired velocity.
    *
    * @param speeds Speeds in meters/sec
    */
-  public Command runVelocity(Supplier<ChassisSpeeds> speeds) {
+  public Command runVelocityCmd(Supplier<ChassisSpeeds> speeds) {
     return this.run(
-        () -> {
-          // Calculate module setpoints
-          ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds.get(), 0.02);
-          SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
-          SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
-
-          // Send setpoints to modules
-          SwerveModuleState[] optimizedSetpointStates =
-              Streams.zip(
-                      Arrays.stream(modules),
-                      Arrays.stream(setpointStates),
-                      (m, s) -> m.runSetpoint(s))
-                  .toArray(SwerveModuleState[]::new);
-
-          // Log setpoint states
-          Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
-          Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
-        });
+        () -> consumeChassisSpeeds(speeds.get()));
   }
 
   /** Stops the drive. */
-  public Command stop() {
-    return runVelocity(ChassisSpeeds::new);
+  public Command stopCmd() {
+    return runVelocityCmd(ChassisSpeeds::new);
   }
 
   public Command runVelocityFieldRelative(Supplier<ChassisSpeeds> speeds) {
-    return this.runVelocity(
+    return this.runVelocityCmd(
         () -> ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), getRotation()));
   }
 
+ 
   /**
    * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
    * return to their normal orientations the next time a nonzero velocity is requested.
    */
-  public Command stopWithX() {
+  public Command stopWithXCmd() {
     return this.run(
         () -> {
           Rotation2d[] headings =
               (Rotation2d[])
                   Arrays.stream(getModuleTranslations()).map(Translation2d::getAngle).toArray();
           kinematics.resetHeadings(headings);
-          stop();
+          stopCmd();
         });
   }
 
   /** Runs forwards at the commanded voltage. */
-  public Command runCharacterizationVolts(double volts) {
+  public Command runCharacterizationVoltsCmd(double volts) {
     return this.run(() -> Arrays.stream(modules).forEach((mod) -> mod.runCharacterization(volts)));
   }
 
@@ -234,6 +258,12 @@ public ChassisSpeeds getVelocity() {
         getRotation());
   }
 
+  public ChassisSpeeds getRobotRelativeSpeeds(){
+    return kinematics.toChassisSpeeds(
+            (SwerveModuleState[])
+                Arrays.stream(modules).map((m) -> m.getState()).toArray(SwerveModuleState[]::new));
+  }
+
   /** Returns the current odometry pose. */
   @AutoLogOutput(key = "Odometry/Robot")
   public Pose2d getPose() {
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 00000000..3e18a45d
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2024.0.0-beta-6.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2024.0.0-beta-6.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2024.0.0-beta-6.2",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file

From aaec6a8eb08faa5ef547a28e7efde1a379c1628e Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 7 Jan 2024 14:37:30 -0800
Subject: [PATCH 02/10] added default auto command

---
 simgui.json                                   | 20 ++++++
 .../deploy/pathplanner/autos/New Auto.auto    | 25 +++++++
 .../pathplanner/paths/Example Path.path       | 65 +++++++++++++++++++
 src/main/java/frc/robot/Robot.java            |  6 ++
 .../subsystems/swerve/SwerveSubsystem.java    | 15 ++++-
 5 files changed, 128 insertions(+), 3 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path

diff --git a/simgui.json b/simgui.json
index 511e36e7..b30b8d3b 100644
--- a/simgui.json
+++ b/simgui.json
@@ -7,8 +7,28 @@
   "NetworkTables": {
     "transitory": {
       "AdvantageKit": {
+        "RealOutputs": {
+          "Odometry": {
+            "open": true
+          },
+          "open": true
+        },
         "open": true
       }
     }
+  },
+  "NetworkTables Info": {
+    "AdvantageScope@1": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "Clients": {
+      "open": true
+    }
   }
 }
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
new file mode 100644
index 00000000..3a6d0044
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.0,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 00000000..5a654c84
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,65 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 6.5
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.0,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 6.7282857142857155,
+        "y": 12.004571428571431
+      },
+      "nextControl": {
+        "x": 4.661221351704417,
+        "y": 3.626963573877582
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.0,
+        "y": 1.0
+      },
+      "prevControl": {
+        "x": 12.442,
+        "y": 6.521714285714287
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 2381f406..5934ca8c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -21,6 +21,8 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
 public class Robot extends LoggedRobot {
   public static enum RobotMode {
     SIM,
@@ -117,6 +119,10 @@ public void autonomousInit() {
     }
   }
 
+  public Command getAutonomousCommand() {
+    return new PathPlannerAuto("New Auto");
+  }
+
   @Override
   public void autonomousPeriodic() {}
 
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 2bc11c8c..35fda621 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -71,7 +71,10 @@ public class SwerveSubsystem extends SubsystemBase {
 
   public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
     this.gyroIO = gyroIO;
-    modules = (Module[]) Arrays.stream(moduleIOs).map(Module::new).toArray();
+    modules = new Module[moduleIOs.length];
+    for (int i = 0; i < modules.length; i++) {
+      modules[i] = new Module(moduleIOs[i]);
+    }
 
     AutoBuilder.configureHolonomic(
                 this::getPose, // Robot pose supplier
@@ -243,9 +246,14 @@ public double getCharacterizationVelocity() {
 
   /** Returns the module states (turn angles and drive velocitoes) for all of the modules. */
   @AutoLogOutput(key = "SwerveStates/Measured")
-  private SwerveModuleState[] getModuleStates() {
+  private SwerveModuleState[] getModuleStates(ModuleIO... moduleIOs) {
+  
     SwerveModuleState[] states =
-        (SwerveModuleState[]) Arrays.stream(modules).map(Module::getState).toArray();
+    new SwerveModuleState[moduleIOs.length];
+    for (int i = 0; i < modules.length; i++) {
+      modules[i] = new Module(moduleIOs[i]);
+    }
+        
     return states;
   }
 
@@ -258,6 +266,7 @@ public ChassisSpeeds getVelocity() {
         getRotation());
   }
 
+  @AutoLogOutput(key = "Odometry/RobotRelativeVelocity")
   public ChassisSpeeds getRobotRelativeSpeeds(){
     return kinematics.toChassisSpeeds(
             (SwerveModuleState[])

From 957a0c6bbb93a57a6a563e537a13a65896f5462c Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 7 Jan 2024 15:41:14 -0800
Subject: [PATCH 03/10] changed getModuleStates, stopWithXCmd, and line 75 to
 be from old commit. set a default autonomous command

---
 src/main/java/frc/robot/Robot.java             |  2 +-
 .../subsystems/swerve/SwerveSubsystem.java     | 18 +++++++++---------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5934ca8c..5a9d0759 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -112,7 +112,7 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
-    autonomousCommand = Commands.none();
+    autonomousCommand = getAutonomousCommand();
 
     if (autonomousCommand != null) {
       autonomousCommand.schedule();
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 35fda621..733d4a7f 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -72,7 +72,7 @@ public class SwerveSubsystem extends SubsystemBase {
   public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
     this.gyroIO = gyroIO;
     modules = new Module[moduleIOs.length];
-    for (int i = 0; i < modules.length; i++) {
+    for (int i = 0; i < moduleIOs.length; i++) {
       modules[i] = new Module(moduleIOs[i]);
     }
 
@@ -222,9 +222,10 @@ public Command runVelocityFieldRelative(Supplier<ChassisSpeeds> speeds) {
   public Command stopWithXCmd() {
     return this.run(
         () -> {
-          Rotation2d[] headings =
-              (Rotation2d[])
-                  Arrays.stream(getModuleTranslations()).map(Translation2d::getAngle).toArray();
+          Rotation2d[] headings = new Rotation2d[4];
+          for (int i = 0; i < modules.length; i++) {
+            headings[i] = getModuleTranslations()[i].getAngle();
+          }
           kinematics.resetHeadings(headings);
           stopCmd();
         });
@@ -246,12 +247,11 @@ public double getCharacterizationVelocity() {
 
   /** Returns the module states (turn angles and drive velocitoes) for all of the modules. */
   @AutoLogOutput(key = "SwerveStates/Measured")
-  private SwerveModuleState[] getModuleStates(ModuleIO... moduleIOs) {
+  private SwerveModuleState[] getModuleStates() {
   
-    SwerveModuleState[] states =
-    new SwerveModuleState[moduleIOs.length];
-    for (int i = 0; i < modules.length; i++) {
-      modules[i] = new Module(moduleIOs[i]);
+    SwerveModuleState[] states = new SwerveModuleState[4];
+    for (int i = 0; i < 4; i++) {
+      states[i] = modules[i].getState();
     }
         
     return states;

From efb93b7a719e69bdc8e4ade5dede73964ad76090 Mon Sep 17 00:00:00 2001
From: stoopipoopy <stoopipoopy@users.noreply.github.com>
Date: Sun, 7 Jan 2024 23:54:11 +0000
Subject: [PATCH 04/10] run spotless

---
 src/main/java/frc/robot/Robot.java            |  4 +-
 .../subsystems/swerve/SwerveSubsystem.java    | 73 +++++++++----------
 2 files changed, 36 insertions(+), 41 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5a9d0759..8820597b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,12 +4,12 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.swerve.GyroIO;
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
@@ -21,8 +21,6 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
-import com.pathplanner.lib.commands.PathPlannerAuto;
-
 public class Robot extends LoggedRobot {
   public static enum RobotMode {
     SIM,
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 733d4a7f..98cbee12 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -18,7 +18,6 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -35,7 +34,6 @@
 import java.util.Arrays;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
-import java.util.function.Consumer;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -77,18 +75,21 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
     }
 
     AutoBuilder.configureHolonomic(
-                this::getPose, // Robot pose supplier
-                this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
-                this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                this::consumeChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
-                new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
-                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants
-                        4.5, // Max module speed, in m/s
-                        0.4, // Drive base radius in meters. Distance from robot center to furthest module.
-                        new ReplanningConfig() // Default path replanning config. See the API for the options here
-                ),
-                this // Reference to this subsystem to set requirements
+        this::getPose, // Robot pose supplier
+        this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
+        this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+        this::consumeChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE
+        // ChassisSpeeds
+        new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in
+            // your Constants class
+            new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+            new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants
+            4.5, // Max module speed, in m/s
+            0.4, // Drive base radius in meters. Distance from robot center to furthest module.
+            new ReplanningConfig() // Default path replanning config. See the API for the options
+            // here
+            ),
+        this // Reference to this subsystem to set requirements
         );
   }
 
@@ -175,23 +176,21 @@ public void periodic() {
     }
   }
 
-  private void consumeChassisSpeeds(ChassisSpeeds speeds){
+  private void consumeChassisSpeeds(ChassisSpeeds speeds) {
     // Calculate module setpoints
-        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
-        SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
-        SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
-
-        // Send setpoints to modules
-        SwerveModuleState[] optimizedSetpointStates =
-            Streams.zip(
-                    Arrays.stream(modules),
-                    Arrays.stream(setpointStates),
-                    (m, s) -> m.runSetpoint(s))
-                .toArray(SwerveModuleState[]::new);
-
-        // Log setpoint states
-        Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
-        Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
+    ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
+    SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
+    SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
+
+    // Send setpoints to modules
+    SwerveModuleState[] optimizedSetpointStates =
+        Streams.zip(
+                Arrays.stream(modules), Arrays.stream(setpointStates), (m, s) -> m.runSetpoint(s))
+            .toArray(SwerveModuleState[]::new);
+
+    // Log setpoint states
+    Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
+    Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
   }
 
   /**
@@ -200,8 +199,7 @@ private void consumeChassisSpeeds(ChassisSpeeds speeds){
    * @param speeds Speeds in meters/sec
    */
   public Command runVelocityCmd(Supplier<ChassisSpeeds> speeds) {
-    return this.run(
-        () -> consumeChassisSpeeds(speeds.get()));
+    return this.run(() -> consumeChassisSpeeds(speeds.get()));
   }
 
   /** Stops the drive. */
@@ -214,7 +212,6 @@ public Command runVelocityFieldRelative(Supplier<ChassisSpeeds> speeds) {
         () -> ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), getRotation()));
   }
 
- 
   /**
    * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
    * return to their normal orientations the next time a nonzero velocity is requested.
@@ -248,12 +245,12 @@ public double getCharacterizationVelocity() {
   /** Returns the module states (turn angles and drive velocitoes) for all of the modules. */
   @AutoLogOutput(key = "SwerveStates/Measured")
   private SwerveModuleState[] getModuleStates() {
-  
+
     SwerveModuleState[] states = new SwerveModuleState[4];
     for (int i = 0; i < 4; i++) {
       states[i] = modules[i].getState();
     }
-        
+
     return states;
   }
 
@@ -267,10 +264,10 @@ public ChassisSpeeds getVelocity() {
   }
 
   @AutoLogOutput(key = "Odometry/RobotRelativeVelocity")
-  public ChassisSpeeds getRobotRelativeSpeeds(){
+  public ChassisSpeeds getRobotRelativeSpeeds() {
     return kinematics.toChassisSpeeds(
-            (SwerveModuleState[])
-                Arrays.stream(modules).map((m) -> m.getState()).toArray(SwerveModuleState[]::new));
+        (SwerveModuleState[])
+            Arrays.stream(modules).map((m) -> m.getState()).toArray(SwerveModuleState[]::new));
   }
 
   /** Returns the current odometry pose. */

From 5e50022fa792f87a9af8d377e97b93348c182e81 Mon Sep 17 00:00:00 2001
From: Lewy Seiden <lewy@seiden.us>
Date: Mon, 8 Jan 2024 20:51:33 -0800
Subject: [PATCH 05/10] added starter autos

---
 choreo.chor                                   | 578 ++++++++++++++++++
 .../deploy/choreo/amp 4 local sgmt 1.traj     | 175 ++++++
 .../deploy/choreo/amp 4 local sgmt 2.traj     | 103 ++++
 .../deploy/choreo/amp 4 local sgmt 3.traj     | 175 ++++++
 src/main/deploy/choreo/amp 4 local.traj       | 454 ++++++++++++++
 .../deploy/pathplanner/autos/local 4.auto     | 100 +++
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 7 files changed, 1586 insertions(+), 1 deletion(-)
 create mode 100644 choreo.chor
 create mode 100644 src/main/deploy/choreo/amp 4 local sgmt 1.traj
 create mode 100644 src/main/deploy/choreo/amp 4 local sgmt 2.traj
 create mode 100644 src/main/deploy/choreo/amp 4 local sgmt 3.traj
 create mode 100644 src/main/deploy/choreo/amp 4 local.traj
 create mode 100644 src/main/deploy/pathplanner/autos/local 4.auto

diff --git a/choreo.chor b/choreo.chor
new file mode 100644
index 00000000..eff8a597
--- /dev/null
+++ b/choreo.chor
@@ -0,0 +1,578 @@
+{
+    "version": "v0.2",
+    "robotConfiguration": {
+        "mass": 37.04398850154597,
+        "rotationalInertia": 2.8,
+        "motorMaxTorque": 1.1526315789473685,
+        "motorMaxVelocity": 4864,
+        "gearing": 6.86,
+        "wheelbase": 0.5206997188221518,
+        "trackWidth": 0.5206997188221518,
+        "bumperLength": 0.8508995405142481,
+        "bumperWidth": 0.8508995405142481,
+        "wheelRadius": 0.050799972568014815
+    },
+    "paths": {
+        "amp 4 local": {
+            "waypoints": [
+                {
+                    "x": 0.7470832467079163,
+                    "y": 6.625458717346191,
+                    "heading": 1.056495497952334,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 2.2610747814178467,
+                    "y": 6.9050798416137695,
+                    "heading": 0.15617120010566338,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 1.3818649053573608,
+                    "y": 5.5012993812561035,
+                    "heading": 0.05628563945524819,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 6
+                },
+                {
+                    "x": 2.3054046630859375,
+                    "y": 5.538240909576416,
+                    "heading": -0.020267736871838693,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 5
+                },
+                {
+                    "x": 1.3892531394958496,
+                    "y": 5.486522674560547,
+                    "heading": 0.03752685285960867,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.4143476486206055,
+                    "y": 4.51963472366333,
+                    "heading": -0.6391760227970993,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 0.7907993793487549,
+                    "y": 4.415216445922852,
+                    "heading": -1.0048134836944498,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0.7470832467079163,
+                    "y": 6.625458717346191,
+                    "heading": 1.056495497952334,
+                    "angularVelocity": 1.8216849722605286e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7875510590073889,
+                    "y": 6.647513625582565,
+                    "heading": 0.9955054911508737,
+                    "angularVelocity": -1.1259767405457384,
+                    "velocityX": 0.7471029727592392,
+                    "velocityY": 0.4071702069138493,
+                    "timestamp": 0.05416631143899068
+                },
+                {
+                    "x": 0.8691098883744304,
+                    "y": 6.691702168377954,
+                    "heading": 0.8785332104893699,
+                    "angularVelocity": -2.159502420489772,
+                    "velocityX": 1.505711339767041,
+                    "velocityY": 0.8157938323926509,
+                    "timestamp": 0.10833262287798136
+                },
+                {
+                    "x": 0.9931586796399057,
+                    "y": 6.758536729383142,
+                    "heading": 0.7242571551382772,
+                    "angularVelocity": -2.848192008142514,
+                    "velocityX": 2.290146549949145,
+                    "velocityY": 1.2338769103830776,
+                    "timestamp": 0.16249893431697204
+                },
+                {
+                    "x": 1.1542844409320208,
+                    "y": 6.847841004747936,
+                    "heading": 0.6582721283096681,
+                    "angularVelocity": -1.2181930996525292,
+                    "velocityX": 2.974648947133801,
+                    "velocityY": 1.6487051267166433,
+                    "timestamp": 0.21666524575596272
+                },
+                {
+                    "x": 1.3362429767095947,
+                    "y": 6.9407596540671515,
+                    "heading": 0.6582720385538592,
+                    "angularVelocity": -0.0000016570411839532803,
+                    "velocityX": 3.3592565368332274,
+                    "velocityY": 1.7154324680917685,
+                    "timestamp": 0.2708315571949534
+                },
+                {
+                    "x": 1.5182021069161247,
+                    "y": 7.033677139333042,
+                    "heading": 0.6582719487984567,
+                    "angularVelocity": -0.0000016570336808247504,
+                    "velocityX": 3.359267510978228,
+                    "velocityY": 1.7154109777356332,
+                    "timestamp": 0.3249978686339441
+                },
+                {
+                    "x": 1.7157951910460691,
+                    "y": 7.085636206134739,
+                    "heading": 0.6582717690726799,
+                    "angularVelocity": -0.0000033180361009936695,
+                    "velocityX": 3.647896245482769,
+                    "velocityY": 0.9592506009979895,
+                    "timestamp": 0.3791641800729348
+                },
+                {
+                    "x": 1.8967940454930174,
+                    "y": 7.095126843263903,
+                    "heading": 0.5939474144774982,
+                    "angularVelocity": -1.1875343342814295,
+                    "velocityX": 3.3415392268460664,
+                    "velocityY": 0.1752129114394957,
+                    "timestamp": 0.4333304915119255
+                },
+                {
+                    "x": 2.0516314923077155,
+                    "y": 7.067103472452243,
+                    "heading": 0.46543090394591263,
+                    "angularVelocity": -2.3726280619336353,
+                    "velocityX": 2.8585562262089095,
+                    "velocityY": -0.5173579309202905,
+                    "timestamp": 0.4874968029509162
+                },
+                {
+                    "x": 2.1749652200442315,
+                    "y": 7.00220729212556,
+                    "heading": 0.30618928904490067,
+                    "angularVelocity": -2.9398644779490204,
+                    "velocityX": 2.2769452905322995,
+                    "velocityY": -1.198091186249191,
+                    "timestamp": 0.5416631143899069
+                },
+                {
+                    "x": 2.2610747814178467,
+                    "y": 6.9050798416137695,
+                    "heading": 0.15617120010566338,
+                    "angularVelocity": -2.7695828819396024,
+                    "velocityX": 1.5897254047029348,
+                    "velocityY": -1.7931339227554435,
+                    "timestamp": 0.5958294258288975
+                },
+                {
+                    "x": 2.3021420961741277,
+                    "y": 6.690129168178249,
+                    "heading": -0.009663328888242836,
+                    "angularVelocity": -2.0165575251161725,
+                    "velocityX": 0.4993809377969598,
+                    "velocityY": -2.6138127003762723,
+                    "timestamp": 0.6780658742953757
+                },
+                {
+                    "x": 2.248466001407306,
+                    "y": 6.41546750147008,
+                    "heading": -0.10873685342712126,
+                    "angularVelocity": -1.204739825057738,
+                    "velocityX": -0.6527044366306908,
+                    "velocityY": -3.3399018541045957,
+                    "timestamp": 0.7603023227618538
+                },
+                {
+                    "x": 2.085165640858422,
+                    "y": 6.151744237063873,
+                    "heading": -0.10873683701731249,
+                    "angularVelocity": 1.9954422992985155e-7,
+                    "velocityX": -1.9857418893211336,
+                    "velocityY": -3.2068902454330503,
+                    "timestamp": 0.842538771228332
+                },
+                {
+                    "x": 1.8574534262453974,
+                    "y": 5.941116320042222,
+                    "heading": -0.10873681487910887,
+                    "angularVelocity": 2.692018447413834e-7,
+                    "velocityX": -2.7689937838918954,
+                    "velocityY": -2.5612477307736583,
+                    "timestamp": 0.9247752196948101
+                },
+                {
+                    "x": 1.629741023592729,
+                    "y": 5.730488606312599,
+                    "heading": -0.10873679274080142,
+                    "angularVelocity": 2.6920310704244503e-7,
+                    "velocityX": -2.76899607046491,
+                    "velocityY": -2.5612452587307564,
+                    "timestamp": 1.0070116681612884
+                },
+                {
+                    "x": 1.4644902813281564,
+                    "y": 5.577695792451846,
+                    "heading": 0.0012781207724603802,
+                    "angularVelocity": 1.337787751839788,
+                    "velocityX": -2.009458644507651,
+                    "velocityY": -1.8579695099921079,
+                    "timestamp": 1.0892481166277665
+                },
+                {
+                    "x": 1.3818649053573608,
+                    "y": 5.5012993812561035,
+                    "heading": 0.05628563945524819,
+                    "angularVelocity": 0.6688946289455666,
+                    "velocityX": -1.0047293810903806,
+                    "velocityY": -0.9289848068631428,
+                    "timestamp": 1.1714845650942447
+                },
+                {
+                    "x": 1.3818649053573608,
+                    "y": 5.5012993812561035,
+                    "heading": 0.05628563945524819,
+                    "angularVelocity": 8.108539434054397e-29,
+                    "velocityX": -3.383351472481295e-31,
+                    "velocityY": 2.5568610146599268e-30,
+                    "timestamp": 1.2537210135607229
+                },
+                {
+                    "x": 1.4659695573316451,
+                    "y": 5.505361506655373,
+                    "heading": 0.050154371824367786,
+                    "angularVelocity": -0.0866134750258778,
+                    "velocityX": 1.188106051128092,
+                    "velocityY": 0.05738369583633677,
+                    "timestamp": 1.3245098559325195
+                },
+                {
+                    "x": 1.6341788550204006,
+                    "y": 5.5134857557639885,
+                    "heading": 0.03789188378734141,
+                    "angularVelocity": -0.17322628293059847,
+                    "velocityX": 2.3762120138268115,
+                    "velocityY": 0.11476736779992304,
+                    "timestamp": 1.395298698304316
+                },
+                {
+                    "x": 1.8864927796012139,
+                    "y": 5.525672125057411,
+                    "heading": 0.019498324963128963,
+                    "angularVelocity": -0.25983697724008453,
+                    "velocityX": 3.5643177106303305,
+                    "velocityY": 0.1721509899740557,
+                    "timestamp": 1.4660875406761127
+                },
+                {
+                    "x": 2.1102347064408926,
+                    "y": 5.533923842143074,
+                    "heading": 0.00011174721946555298,
+                    "angularVelocity": -0.2738648789005671,
+                    "velocityX": 3.160694812108141,
+                    "velocityY": 0.11656804673148825,
+                    "timestamp": 1.5368763830479093
+                },
+                {
+                    "x": 2.249872007587195,
+                    "y": 5.538113436672261,
+                    "heading": -0.013143603106743272,
+                    "angularVelocity": -0.18725197195045482,
+                    "velocityX": 1.9725891322378306,
+                    "velocityY": 0.0591843910539215,
+                    "timestamp": 1.607665225419706
+                },
+                {
+                    "x": 2.3054046630859375,
+                    "y": 5.538240909576416,
+                    "heading": -0.020267736871838693,
+                    "angularVelocity": -0.10063921836266371,
+                    "velocityX": 0.784483170484331,
+                    "velocityY": 0.0018007485344283746,
+                    "timestamp": 1.6784540677915025
+                },
+                {
+                    "x": 2.206836259942782,
+                    "y": 5.529745448091516,
+                    "heading": -0.017578724891835627,
+                    "angularVelocity": 0.025983583153922206,
+                    "velocityX": -0.9524540308729255,
+                    "velocityY": -0.08209057139403513,
+                    "timestamp": 1.7819429501397848
+                },
+                {
+                    "x": 1.9285141868544358,
+                    "y": 5.512568166846942,
+                    "heading": -0.0017855827044546864,
+                    "angularVelocity": 0.15260713836130316,
+                    "velocityX": -2.689391041558251,
+                    "velocityY": -0.1659818992610695,
+                    "timestamp": 1.885431832488067
+                },
+                {
+                    "x": 1.5690068282385219,
+                    "y": 5.495204505355553,
+                    "heading": 0.024422680398419737,
+                    "angularVelocity": 0.25324713638971313,
+                    "velocityX": -3.4738742023131053,
+                    "velocityY": -0.16778286804715686,
+                    "timestamp": 1.9889207148363492
+                },
+                {
+                    "x": 1.3892531394958496,
+                    "y": 5.486522674560547,
+                    "heading": 0.03752685285960867,
+                    "angularVelocity": 0.12662396350062077,
+                    "velocityX": -1.7369371923230166,
+                    "velocityY": -0.08389143450007014,
+                    "timestamp": 2.0924095971846315
+                },
+                {
+                    "x": 1.3892531394958496,
+                    "y": 5.486522674560547,
+                    "heading": 0.03752685285960867,
+                    "angularVelocity": 2.746210792067846e-31,
+                    "velocityX": -7.082594657438039e-35,
+                    "velocityY": 0,
+                    "timestamp": 2.195898479532914
+                },
+                {
+                    "x": 1.4239336560949027,
+                    "y": 5.45678487802077,
+                    "heading": -0.004824083437490689,
+                    "angularVelocity": -0.7980299015185681,
+                    "velocityX": 0.6534941530454559,
+                    "velocityY": -0.5603571707963146,
+                    "timestamp": 2.248967839736724
+                },
+                {
+                    "x": 1.4936387312915242,
+                    "y": 5.397060158439441,
+                    "heading": -0.08394241765705696,
+                    "angularVelocity": -1.4908477116686103,
+                    "velocityX": 1.3134711805253199,
+                    "velocityY": -1.1254086982009737,
+                    "timestamp": 2.3020371999405342
+                },
+                {
+                    "x": 1.5989756629525658,
+                    "y": 5.306833356180067,
+                    "heading": -0.18501626934212806,
+                    "angularVelocity": -1.9045613381601734,
+                    "velocityX": 1.9848916824416323,
+                    "velocityY": -1.7001675149815771,
+                    "timestamp": 2.3551065601443444
+                },
+                {
+                    "x": 1.7389822594115079,
+                    "y": 5.188268009617622,
+                    "heading": -0.23253583212093973,
+                    "angularVelocity": -0.8954236982755346,
+                    "velocityX": 2.6381813521258652,
+                    "velocityY": -2.234158205546495,
+                    "timestamp": 2.4081759203481545
+                },
+                {
+                    "x": 1.8903539316186226,
+                    "y": 5.05728781912084,
+                    "heading": -0.23253589895082247,
+                    "angularVelocity": -0.0000012592931680036756,
+                    "velocityX": 2.85233648240303,
+                    "velocityY": -2.4680943955939854,
+                    "timestamp": 2.4612452805519647
+                },
+                {
+                    "x": 2.041725559967012,
+                    "y": 4.926307577937187,
+                    "heading": -0.23253596578056132,
+                    "angularVelocity": -0.000001259290456718511,
+                    "velocityX": 2.8523356559614563,
+                    "velocityY": -2.468095350700103,
+                    "timestamp": 2.514314640755775
+                },
+                {
+                    "x": 2.186666927649336,
+                    "y": 4.788245829195716,
+                    "heading": -0.23253707199818868,
+                    "angularVelocity": -0.000020844751532055486,
+                    "velocityX": 2.731168552356477,
+                    "velocityY": -2.601534071849594,
+                    "timestamp": 2.567384000959585
+                },
+                {
+                    "x": 2.299267843418341,
+                    "y": 4.670864736575426,
+                    "heading": -0.33873841006148575,
+                    "angularVelocity": -2.0011799210587196,
+                    "velocityX": 2.1217688575209404,
+                    "velocityY": -2.2118429950821876,
+                    "timestamp": 2.620453361163395
+                },
+                {
+                    "x": 2.37528105951498,
+                    "y": 4.58047733131235,
+                    "heading": -0.49175658993248267,
+                    "angularVelocity": -2.883362062088883,
+                    "velocityX": 1.4323371490576546,
+                    "velocityY": -1.7031937998865565,
+                    "timestamp": 2.6735227213672053
+                },
+                {
+                    "x": 2.4143476486206055,
+                    "y": 4.51963472366333,
+                    "heading": -0.6391760227970993,
+                    "angularVelocity": -2.777863390447102,
+                    "velocityX": 0.7361420781330721,
+                    "velocityY": -1.146473358927959,
+                    "timestamp": 2.7265920815710154
+                },
+                {
+                    "x": 2.3701336666128094,
+                    "y": 4.4395971700859445,
+                    "heading": -0.8245153775471664,
+                    "angularVelocity": -2.372308285003151,
+                    "velocityX": -0.5659305114745719,
+                    "velocityY": -1.0244653744427692,
+                    "timestamp": 2.8047182492034923
+                },
+                {
+                    "x": 2.2242072743116044,
+                    "y": 4.369198833257615,
+                    "heading": -0.9780363443258114,
+                    "angularVelocity": -1.9650390058915468,
+                    "velocityX": -1.8678299054380199,
+                    "velocityY": -0.901085244056695,
+                    "timestamp": 2.8828444168359693
+                },
+                {
+                    "x": 1.9766118523863694,
+                    "y": 4.308771886473381,
+                    "heading": -1.099290083952399,
+                    "angularVelocity": -1.5520246711313,
+                    "velocityX": -3.169174035132216,
+                    "velocityY": -0.7734533590395138,
+                    "timestamp": 2.960970584468446
+                },
+                {
+                    "x": 1.6829782303645726,
+                    "y": 4.333642341587589,
+                    "heading": -1.099290070338212,
+                    "angularVelocity": 1.7425898802224296e-7,
+                    "velocityX": -3.758454189166382,
+                    "velocityY": 0.31833706769275627,
+                    "timestamp": 3.039096752100923
+                },
+                {
+                    "x": 1.3895177021759386,
+                    "y": 4.36047817582874,
+                    "heading": -1.0992900564673602,
+                    "angularVelocity": 1.7754424736252278e-7,
+                    "velocityX": -3.7562386212150933,
+                    "velocityY": 0.34349354453672953,
+                    "timestamp": 3.1172229197334
+                },
+                {
+                    "x": 1.0960571742019716,
+                    "y": 4.38731401241607,
+                    "heading": -1.0992900425963372,
+                    "angularVelocity": 1.7754644295782002e-7,
+                    "velocityX": -3.756238618467393,
+                    "velocityY": 0.34349357456735763,
+                    "timestamp": 3.195349087365877
+                },
+                {
+                    "x": 0.8925519822862651,
+                    "y": 4.405915634394149,
+                    "heading": -1.0363057213411215,
+                    "angularVelocity": 0.8061872630368462,
+                    "velocityX": -2.6048275255614968,
+                    "velocityY": 0.23809720279105512,
+                    "timestamp": 3.273475254998354
+                },
+                {
+                    "x": 0.7907993793487549,
+                    "y": 4.415216445922852,
+                    "heading": -1.0048134836944498,
+                    "angularVelocity": 0.4030946173477082,
+                    "velocityX": -1.302413852119022,
+                    "velocityY": 0.11904860830311666,
+                    "timestamp": 3.351601422630831
+                },
+                {
+                    "x": 0.7907993793487549,
+                    "y": 4.415216445922852,
+                    "heading": -1.0048134836944498,
+                    "angularVelocity": -9.422070435540628e-32,
+                    "velocityX": 1.9369819218763315e-32,
+                    "velocityY": -1.162688749272308e-31,
+                    "timestamp": 3.4297275902633078
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "97aec77a-72c9-452d-b090-247d1b380727"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "2e8488aa-d4fb-46f3-93c2-d32a1ab6c3d0"
+                },
+                {
+                    "scope": [
+                        2
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "daeb5c61-2b20-430d-951f-f2bdd4f2e4c0"
+                },
+                {
+                    "scope": [
+                        4
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "96ea1bb1-66cd-462e-bee5-77ef8d7233f0"
+                },
+                {
+                    "scope": [
+                        5
+                    ],
+                    "type": "WptVelocityDirection",
+                    "uuid": "889873f8-2ca0-445b-af07-7765055f2cb4",
+                    "direction": -1
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local sgmt 1.traj b/src/main/deploy/choreo/amp 4 local sgmt 1.traj
new file mode 100644
index 00000000..ddf10560
--- /dev/null
+++ b/src/main/deploy/choreo/amp 4 local sgmt 1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 0.7470832467079163,
+            "y": 6.625458717346191,
+            "heading": 1.056495497952334,
+            "angularVelocity": 1.8216849722605286e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7875510590073889,
+            "y": 6.647513625582565,
+            "heading": 0.9955054911508737,
+            "angularVelocity": -1.1259767405457384,
+            "velocityX": 0.7471029727592392,
+            "velocityY": 0.4071702069138493,
+            "timestamp": 0.05416631143899068
+        },
+        {
+            "x": 0.8691098883744304,
+            "y": 6.691702168377954,
+            "heading": 0.8785332104893699,
+            "angularVelocity": -2.159502420489772,
+            "velocityX": 1.505711339767041,
+            "velocityY": 0.8157938323926509,
+            "timestamp": 0.10833262287798136
+        },
+        {
+            "x": 0.9931586796399057,
+            "y": 6.758536729383142,
+            "heading": 0.7242571551382772,
+            "angularVelocity": -2.848192008142514,
+            "velocityX": 2.290146549949145,
+            "velocityY": 1.2338769103830776,
+            "timestamp": 0.16249893431697204
+        },
+        {
+            "x": 1.1542844409320208,
+            "y": 6.847841004747936,
+            "heading": 0.6582721283096681,
+            "angularVelocity": -1.2181930996525292,
+            "velocityX": 2.974648947133801,
+            "velocityY": 1.6487051267166433,
+            "timestamp": 0.21666524575596272
+        },
+        {
+            "x": 1.3362429767095947,
+            "y": 6.9407596540671515,
+            "heading": 0.6582720385538592,
+            "angularVelocity": -0.0000016570411839532803,
+            "velocityX": 3.3592565368332274,
+            "velocityY": 1.7154324680917685,
+            "timestamp": 0.2708315571949534
+        },
+        {
+            "x": 1.5182021069161247,
+            "y": 7.033677139333042,
+            "heading": 0.6582719487984567,
+            "angularVelocity": -0.0000016570336808247504,
+            "velocityX": 3.359267510978228,
+            "velocityY": 1.7154109777356332,
+            "timestamp": 0.3249978686339441
+        },
+        {
+            "x": 1.7157951910460691,
+            "y": 7.085636206134739,
+            "heading": 0.6582717690726799,
+            "angularVelocity": -0.0000033180361009936695,
+            "velocityX": 3.647896245482769,
+            "velocityY": 0.9592506009979895,
+            "timestamp": 0.3791641800729348
+        },
+        {
+            "x": 1.8967940454930174,
+            "y": 7.095126843263903,
+            "heading": 0.5939474144774982,
+            "angularVelocity": -1.1875343342814295,
+            "velocityX": 3.3415392268460664,
+            "velocityY": 0.1752129114394957,
+            "timestamp": 0.4333304915119255
+        },
+        {
+            "x": 2.0516314923077155,
+            "y": 7.067103472452243,
+            "heading": 0.46543090394591263,
+            "angularVelocity": -2.3726280619336353,
+            "velocityX": 2.8585562262089095,
+            "velocityY": -0.5173579309202905,
+            "timestamp": 0.4874968029509162
+        },
+        {
+            "x": 2.1749652200442315,
+            "y": 7.00220729212556,
+            "heading": 0.30618928904490067,
+            "angularVelocity": -2.9398644779490204,
+            "velocityX": 2.2769452905322995,
+            "velocityY": -1.198091186249191,
+            "timestamp": 0.5416631143899069
+        },
+        {
+            "x": 2.2610747814178467,
+            "y": 6.9050798416137695,
+            "heading": 0.15617120010566338,
+            "angularVelocity": -2.7695828819396024,
+            "velocityX": 1.5897254047029348,
+            "velocityY": -1.7931339227554435,
+            "timestamp": 0.5958294258288975
+        },
+        {
+            "x": 2.3021420961741277,
+            "y": 6.690129168178249,
+            "heading": -0.009663328888242836,
+            "angularVelocity": -2.0165575251161725,
+            "velocityX": 0.4993809377969598,
+            "velocityY": -2.6138127003762723,
+            "timestamp": 0.6780658742953757
+        },
+        {
+            "x": 2.248466001407306,
+            "y": 6.41546750147008,
+            "heading": -0.10873685342712126,
+            "angularVelocity": -1.204739825057738,
+            "velocityX": -0.6527044366306908,
+            "velocityY": -3.3399018541045957,
+            "timestamp": 0.7603023227618538
+        },
+        {
+            "x": 2.085165640858422,
+            "y": 6.151744237063873,
+            "heading": -0.10873683701731249,
+            "angularVelocity": 1.9954422992985155e-7,
+            "velocityX": -1.9857418893211336,
+            "velocityY": -3.2068902454330503,
+            "timestamp": 0.842538771228332
+        },
+        {
+            "x": 1.8574534262453974,
+            "y": 5.941116320042222,
+            "heading": -0.10873681487910887,
+            "angularVelocity": 2.692018447413834e-7,
+            "velocityX": -2.7689937838918954,
+            "velocityY": -2.5612477307736583,
+            "timestamp": 0.9247752196948101
+        },
+        {
+            "x": 1.629741023592729,
+            "y": 5.730488606312599,
+            "heading": -0.10873679274080142,
+            "angularVelocity": 2.6920310704244503e-7,
+            "velocityX": -2.76899607046491,
+            "velocityY": -2.5612452587307564,
+            "timestamp": 1.0070116681612884
+        },
+        {
+            "x": 1.4644902813281564,
+            "y": 5.577695792451846,
+            "heading": 0.0012781207724603802,
+            "angularVelocity": 1.337787751839788,
+            "velocityX": -2.009458644507651,
+            "velocityY": -1.8579695099921079,
+            "timestamp": 1.0892481166277665
+        },
+        {
+            "x": 1.3818649053573608,
+            "y": 5.5012993812561035,
+            "heading": 0.05628563945524819,
+            "angularVelocity": 0.6688946289455666,
+            "velocityX": -1.0047293810903806,
+            "velocityY": -0.9289848068631428,
+            "timestamp": 1.1714845650942447
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj
new file mode 100644
index 00000000..88093dbe
--- /dev/null
+++ b/src/main/deploy/choreo/amp 4 local sgmt 2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 1.3818649053573608,
+            "y": 5.5012993812561035,
+            "heading": 0.05628563945524819,
+            "angularVelocity": 8.108539434054397e-29,
+            "velocityX": -3.383351472481295e-31,
+            "velocityY": 2.5568610146599268e-30,
+            "timestamp": 1.2537210135607229
+        },
+        {
+            "x": 1.4659695573316451,
+            "y": 5.505361506655373,
+            "heading": 0.050154371824367786,
+            "angularVelocity": -0.0866134750258778,
+            "velocityX": 1.188106051128092,
+            "velocityY": 0.05738369583633677,
+            "timestamp": 1.3245098559325195
+        },
+        {
+            "x": 1.6341788550204006,
+            "y": 5.5134857557639885,
+            "heading": 0.03789188378734141,
+            "angularVelocity": -0.17322628293059847,
+            "velocityX": 2.3762120138268115,
+            "velocityY": 0.11476736779992304,
+            "timestamp": 1.395298698304316
+        },
+        {
+            "x": 1.8864927796012139,
+            "y": 5.525672125057411,
+            "heading": 0.019498324963128963,
+            "angularVelocity": -0.25983697724008453,
+            "velocityX": 3.5643177106303305,
+            "velocityY": 0.1721509899740557,
+            "timestamp": 1.4660875406761127
+        },
+        {
+            "x": 2.1102347064408926,
+            "y": 5.533923842143074,
+            "heading": 0.00011174721946555298,
+            "angularVelocity": -0.2738648789005671,
+            "velocityX": 3.160694812108141,
+            "velocityY": 0.11656804673148825,
+            "timestamp": 1.5368763830479093
+        },
+        {
+            "x": 2.249872007587195,
+            "y": 5.538113436672261,
+            "heading": -0.013143603106743272,
+            "angularVelocity": -0.18725197195045482,
+            "velocityX": 1.9725891322378306,
+            "velocityY": 0.0591843910539215,
+            "timestamp": 1.607665225419706
+        },
+        {
+            "x": 2.3054046630859375,
+            "y": 5.538240909576416,
+            "heading": -0.020267736871838693,
+            "angularVelocity": -0.10063921836266371,
+            "velocityX": 0.784483170484331,
+            "velocityY": 0.0018007485344283746,
+            "timestamp": 1.6784540677915025
+        },
+        {
+            "x": 2.206836259942782,
+            "y": 5.529745448091516,
+            "heading": -0.017578724891835627,
+            "angularVelocity": 0.025983583153922206,
+            "velocityX": -0.9524540308729255,
+            "velocityY": -0.08209057139403513,
+            "timestamp": 1.7819429501397848
+        },
+        {
+            "x": 1.9285141868544358,
+            "y": 5.512568166846942,
+            "heading": -0.0017855827044546864,
+            "angularVelocity": 0.15260713836130316,
+            "velocityX": -2.689391041558251,
+            "velocityY": -0.1659818992610695,
+            "timestamp": 1.885431832488067
+        },
+        {
+            "x": 1.5690068282385219,
+            "y": 5.495204505355553,
+            "heading": 0.024422680398419737,
+            "angularVelocity": 0.25324713638971313,
+            "velocityX": -3.4738742023131053,
+            "velocityY": -0.16778286804715686,
+            "timestamp": 1.9889207148363492
+        },
+        {
+            "x": 1.3892531394958496,
+            "y": 5.486522674560547,
+            "heading": 0.03752685285960867,
+            "angularVelocity": 0.12662396350062077,
+            "velocityX": -1.7369371923230166,
+            "velocityY": -0.08389143450007014,
+            "timestamp": 2.0924095971846315
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj
new file mode 100644
index 00000000..3c66e297
--- /dev/null
+++ b/src/main/deploy/choreo/amp 4 local sgmt 3.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 1.3892531394958496,
+            "y": 5.486522674560547,
+            "heading": 0.03752685285960867,
+            "angularVelocity": 2.746210792067846e-31,
+            "velocityX": -7.082594657438039e-35,
+            "velocityY": 0,
+            "timestamp": 2.195898479532914
+        },
+        {
+            "x": 1.4239336560949027,
+            "y": 5.45678487802077,
+            "heading": -0.004824083437490689,
+            "angularVelocity": -0.7980299015185681,
+            "velocityX": 0.6534941530454559,
+            "velocityY": -0.5603571707963146,
+            "timestamp": 2.248967839736724
+        },
+        {
+            "x": 1.4936387312915242,
+            "y": 5.397060158439441,
+            "heading": -0.08394241765705696,
+            "angularVelocity": -1.4908477116686103,
+            "velocityX": 1.3134711805253199,
+            "velocityY": -1.1254086982009737,
+            "timestamp": 2.3020371999405342
+        },
+        {
+            "x": 1.5989756629525658,
+            "y": 5.306833356180067,
+            "heading": -0.18501626934212806,
+            "angularVelocity": -1.9045613381601734,
+            "velocityX": 1.9848916824416323,
+            "velocityY": -1.7001675149815771,
+            "timestamp": 2.3551065601443444
+        },
+        {
+            "x": 1.7389822594115079,
+            "y": 5.188268009617622,
+            "heading": -0.23253583212093973,
+            "angularVelocity": -0.8954236982755346,
+            "velocityX": 2.6381813521258652,
+            "velocityY": -2.234158205546495,
+            "timestamp": 2.4081759203481545
+        },
+        {
+            "x": 1.8903539316186226,
+            "y": 5.05728781912084,
+            "heading": -0.23253589895082247,
+            "angularVelocity": -0.0000012592931680036756,
+            "velocityX": 2.85233648240303,
+            "velocityY": -2.4680943955939854,
+            "timestamp": 2.4612452805519647
+        },
+        {
+            "x": 2.041725559967012,
+            "y": 4.926307577937187,
+            "heading": -0.23253596578056132,
+            "angularVelocity": -0.000001259290456718511,
+            "velocityX": 2.8523356559614563,
+            "velocityY": -2.468095350700103,
+            "timestamp": 2.514314640755775
+        },
+        {
+            "x": 2.186666927649336,
+            "y": 4.788245829195716,
+            "heading": -0.23253707199818868,
+            "angularVelocity": -0.000020844751532055486,
+            "velocityX": 2.731168552356477,
+            "velocityY": -2.601534071849594,
+            "timestamp": 2.567384000959585
+        },
+        {
+            "x": 2.299267843418341,
+            "y": 4.670864736575426,
+            "heading": -0.33873841006148575,
+            "angularVelocity": -2.0011799210587196,
+            "velocityX": 2.1217688575209404,
+            "velocityY": -2.2118429950821876,
+            "timestamp": 2.620453361163395
+        },
+        {
+            "x": 2.37528105951498,
+            "y": 4.58047733131235,
+            "heading": -0.49175658993248267,
+            "angularVelocity": -2.883362062088883,
+            "velocityX": 1.4323371490576546,
+            "velocityY": -1.7031937998865565,
+            "timestamp": 2.6735227213672053
+        },
+        {
+            "x": 2.4143476486206055,
+            "y": 4.51963472366333,
+            "heading": -0.6391760227970993,
+            "angularVelocity": -2.777863390447102,
+            "velocityX": 0.7361420781330721,
+            "velocityY": -1.146473358927959,
+            "timestamp": 2.7265920815710154
+        },
+        {
+            "x": 2.3701336666128094,
+            "y": 4.4395971700859445,
+            "heading": -0.8245153775471664,
+            "angularVelocity": -2.372308285003151,
+            "velocityX": -0.5659305114745719,
+            "velocityY": -1.0244653744427692,
+            "timestamp": 2.8047182492034923
+        },
+        {
+            "x": 2.2242072743116044,
+            "y": 4.369198833257615,
+            "heading": -0.9780363443258114,
+            "angularVelocity": -1.9650390058915468,
+            "velocityX": -1.8678299054380199,
+            "velocityY": -0.901085244056695,
+            "timestamp": 2.8828444168359693
+        },
+        {
+            "x": 1.9766118523863694,
+            "y": 4.308771886473381,
+            "heading": -1.099290083952399,
+            "angularVelocity": -1.5520246711313,
+            "velocityX": -3.169174035132216,
+            "velocityY": -0.7734533590395138,
+            "timestamp": 2.960970584468446
+        },
+        {
+            "x": 1.6829782303645726,
+            "y": 4.333642341587589,
+            "heading": -1.099290070338212,
+            "angularVelocity": 1.7425898802224296e-7,
+            "velocityX": -3.758454189166382,
+            "velocityY": 0.31833706769275627,
+            "timestamp": 3.039096752100923
+        },
+        {
+            "x": 1.3895177021759386,
+            "y": 4.36047817582874,
+            "heading": -1.0992900564673602,
+            "angularVelocity": 1.7754424736252278e-7,
+            "velocityX": -3.7562386212150933,
+            "velocityY": 0.34349354453672953,
+            "timestamp": 3.1172229197334
+        },
+        {
+            "x": 1.0960571742019716,
+            "y": 4.38731401241607,
+            "heading": -1.0992900425963372,
+            "angularVelocity": 1.7754644295782002e-7,
+            "velocityX": -3.756238618467393,
+            "velocityY": 0.34349357456735763,
+            "timestamp": 3.195349087365877
+        },
+        {
+            "x": 0.8925519822862651,
+            "y": 4.405915634394149,
+            "heading": -1.0363057213411215,
+            "angularVelocity": 0.8061872630368462,
+            "velocityX": -2.6048275255614968,
+            "velocityY": 0.23809720279105512,
+            "timestamp": 3.273475254998354
+        },
+        {
+            "x": 0.7907993793487549,
+            "y": 4.415216445922852,
+            "heading": -1.0048134836944498,
+            "angularVelocity": 0.4030946173477082,
+            "velocityX": -1.302413852119022,
+            "velocityY": 0.11904860830311666,
+            "timestamp": 3.351601422630831
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj
new file mode 100644
index 00000000..2864d9ec
--- /dev/null
+++ b/src/main/deploy/choreo/amp 4 local.traj	
@@ -0,0 +1,454 @@
+{
+    "samples": [
+        {
+            "x": 0.7470832467079163,
+            "y": 6.625458717346191,
+            "heading": 1.056495497952334,
+            "angularVelocity": 1.8216849722605286e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7875510590073889,
+            "y": 6.647513625582565,
+            "heading": 0.9955054911508737,
+            "angularVelocity": -1.1259767405457384,
+            "velocityX": 0.7471029727592392,
+            "velocityY": 0.4071702069138493,
+            "timestamp": 0.05416631143899068
+        },
+        {
+            "x": 0.8691098883744304,
+            "y": 6.691702168377954,
+            "heading": 0.8785332104893699,
+            "angularVelocity": -2.159502420489772,
+            "velocityX": 1.505711339767041,
+            "velocityY": 0.8157938323926509,
+            "timestamp": 0.10833262287798136
+        },
+        {
+            "x": 0.9931586796399057,
+            "y": 6.758536729383142,
+            "heading": 0.7242571551382772,
+            "angularVelocity": -2.848192008142514,
+            "velocityX": 2.290146549949145,
+            "velocityY": 1.2338769103830776,
+            "timestamp": 0.16249893431697204
+        },
+        {
+            "x": 1.1542844409320208,
+            "y": 6.847841004747936,
+            "heading": 0.6582721283096681,
+            "angularVelocity": -1.2181930996525292,
+            "velocityX": 2.974648947133801,
+            "velocityY": 1.6487051267166433,
+            "timestamp": 0.21666524575596272
+        },
+        {
+            "x": 1.3362429767095947,
+            "y": 6.9407596540671515,
+            "heading": 0.6582720385538592,
+            "angularVelocity": -0.0000016570411839532803,
+            "velocityX": 3.3592565368332274,
+            "velocityY": 1.7154324680917685,
+            "timestamp": 0.2708315571949534
+        },
+        {
+            "x": 1.5182021069161247,
+            "y": 7.033677139333042,
+            "heading": 0.6582719487984567,
+            "angularVelocity": -0.0000016570336808247504,
+            "velocityX": 3.359267510978228,
+            "velocityY": 1.7154109777356332,
+            "timestamp": 0.3249978686339441
+        },
+        {
+            "x": 1.7157951910460691,
+            "y": 7.085636206134739,
+            "heading": 0.6582717690726799,
+            "angularVelocity": -0.0000033180361009936695,
+            "velocityX": 3.647896245482769,
+            "velocityY": 0.9592506009979895,
+            "timestamp": 0.3791641800729348
+        },
+        {
+            "x": 1.8967940454930174,
+            "y": 7.095126843263903,
+            "heading": 0.5939474144774982,
+            "angularVelocity": -1.1875343342814295,
+            "velocityX": 3.3415392268460664,
+            "velocityY": 0.1752129114394957,
+            "timestamp": 0.4333304915119255
+        },
+        {
+            "x": 2.0516314923077155,
+            "y": 7.067103472452243,
+            "heading": 0.46543090394591263,
+            "angularVelocity": -2.3726280619336353,
+            "velocityX": 2.8585562262089095,
+            "velocityY": -0.5173579309202905,
+            "timestamp": 0.4874968029509162
+        },
+        {
+            "x": 2.1749652200442315,
+            "y": 7.00220729212556,
+            "heading": 0.30618928904490067,
+            "angularVelocity": -2.9398644779490204,
+            "velocityX": 2.2769452905322995,
+            "velocityY": -1.198091186249191,
+            "timestamp": 0.5416631143899069
+        },
+        {
+            "x": 2.2610747814178467,
+            "y": 6.9050798416137695,
+            "heading": 0.15617120010566338,
+            "angularVelocity": -2.7695828819396024,
+            "velocityX": 1.5897254047029348,
+            "velocityY": -1.7931339227554435,
+            "timestamp": 0.5958294258288975
+        },
+        {
+            "x": 2.3021420961741277,
+            "y": 6.690129168178249,
+            "heading": -0.009663328888242836,
+            "angularVelocity": -2.0165575251161725,
+            "velocityX": 0.4993809377969598,
+            "velocityY": -2.6138127003762723,
+            "timestamp": 0.6780658742953757
+        },
+        {
+            "x": 2.248466001407306,
+            "y": 6.41546750147008,
+            "heading": -0.10873685342712126,
+            "angularVelocity": -1.204739825057738,
+            "velocityX": -0.6527044366306908,
+            "velocityY": -3.3399018541045957,
+            "timestamp": 0.7603023227618538
+        },
+        {
+            "x": 2.085165640858422,
+            "y": 6.151744237063873,
+            "heading": -0.10873683701731249,
+            "angularVelocity": 1.9954422992985155e-7,
+            "velocityX": -1.9857418893211336,
+            "velocityY": -3.2068902454330503,
+            "timestamp": 0.842538771228332
+        },
+        {
+            "x": 1.8574534262453974,
+            "y": 5.941116320042222,
+            "heading": -0.10873681487910887,
+            "angularVelocity": 2.692018447413834e-7,
+            "velocityX": -2.7689937838918954,
+            "velocityY": -2.5612477307736583,
+            "timestamp": 0.9247752196948101
+        },
+        {
+            "x": 1.629741023592729,
+            "y": 5.730488606312599,
+            "heading": -0.10873679274080142,
+            "angularVelocity": 2.6920310704244503e-7,
+            "velocityX": -2.76899607046491,
+            "velocityY": -2.5612452587307564,
+            "timestamp": 1.0070116681612884
+        },
+        {
+            "x": 1.4644902813281564,
+            "y": 5.577695792451846,
+            "heading": 0.0012781207724603802,
+            "angularVelocity": 1.337787751839788,
+            "velocityX": -2.009458644507651,
+            "velocityY": -1.8579695099921079,
+            "timestamp": 1.0892481166277665
+        },
+        {
+            "x": 1.3818649053573608,
+            "y": 5.5012993812561035,
+            "heading": 0.05628563945524819,
+            "angularVelocity": 0.6688946289455666,
+            "velocityX": -1.0047293810903806,
+            "velocityY": -0.9289848068631428,
+            "timestamp": 1.1714845650942447
+        },
+        {
+            "x": 1.3818649053573608,
+            "y": 5.5012993812561035,
+            "heading": 0.05628563945524819,
+            "angularVelocity": 8.108539434054397e-29,
+            "velocityX": -3.383351472481295e-31,
+            "velocityY": 2.5568610146599268e-30,
+            "timestamp": 1.2537210135607229
+        },
+        {
+            "x": 1.4659695573316451,
+            "y": 5.505361506655373,
+            "heading": 0.050154371824367786,
+            "angularVelocity": -0.0866134750258778,
+            "velocityX": 1.188106051128092,
+            "velocityY": 0.05738369583633677,
+            "timestamp": 1.3245098559325195
+        },
+        {
+            "x": 1.6341788550204006,
+            "y": 5.5134857557639885,
+            "heading": 0.03789188378734141,
+            "angularVelocity": -0.17322628293059847,
+            "velocityX": 2.3762120138268115,
+            "velocityY": 0.11476736779992304,
+            "timestamp": 1.395298698304316
+        },
+        {
+            "x": 1.8864927796012139,
+            "y": 5.525672125057411,
+            "heading": 0.019498324963128963,
+            "angularVelocity": -0.25983697724008453,
+            "velocityX": 3.5643177106303305,
+            "velocityY": 0.1721509899740557,
+            "timestamp": 1.4660875406761127
+        },
+        {
+            "x": 2.1102347064408926,
+            "y": 5.533923842143074,
+            "heading": 0.00011174721946555298,
+            "angularVelocity": -0.2738648789005671,
+            "velocityX": 3.160694812108141,
+            "velocityY": 0.11656804673148825,
+            "timestamp": 1.5368763830479093
+        },
+        {
+            "x": 2.249872007587195,
+            "y": 5.538113436672261,
+            "heading": -0.013143603106743272,
+            "angularVelocity": -0.18725197195045482,
+            "velocityX": 1.9725891322378306,
+            "velocityY": 0.0591843910539215,
+            "timestamp": 1.607665225419706
+        },
+        {
+            "x": 2.3054046630859375,
+            "y": 5.538240909576416,
+            "heading": -0.020267736871838693,
+            "angularVelocity": -0.10063921836266371,
+            "velocityX": 0.784483170484331,
+            "velocityY": 0.0018007485344283746,
+            "timestamp": 1.6784540677915025
+        },
+        {
+            "x": 2.206836259942782,
+            "y": 5.529745448091516,
+            "heading": -0.017578724891835627,
+            "angularVelocity": 0.025983583153922206,
+            "velocityX": -0.9524540308729255,
+            "velocityY": -0.08209057139403513,
+            "timestamp": 1.7819429501397848
+        },
+        {
+            "x": 1.9285141868544358,
+            "y": 5.512568166846942,
+            "heading": -0.0017855827044546864,
+            "angularVelocity": 0.15260713836130316,
+            "velocityX": -2.689391041558251,
+            "velocityY": -0.1659818992610695,
+            "timestamp": 1.885431832488067
+        },
+        {
+            "x": 1.5690068282385219,
+            "y": 5.495204505355553,
+            "heading": 0.024422680398419737,
+            "angularVelocity": 0.25324713638971313,
+            "velocityX": -3.4738742023131053,
+            "velocityY": -0.16778286804715686,
+            "timestamp": 1.9889207148363492
+        },
+        {
+            "x": 1.3892531394958496,
+            "y": 5.486522674560547,
+            "heading": 0.03752685285960867,
+            "angularVelocity": 0.12662396350062077,
+            "velocityX": -1.7369371923230166,
+            "velocityY": -0.08389143450007014,
+            "timestamp": 2.0924095971846315
+        },
+        {
+            "x": 1.3892531394958496,
+            "y": 5.486522674560547,
+            "heading": 0.03752685285960867,
+            "angularVelocity": 2.746210792067846e-31,
+            "velocityX": -7.082594657438039e-35,
+            "velocityY": 0,
+            "timestamp": 2.195898479532914
+        },
+        {
+            "x": 1.4239336560949027,
+            "y": 5.45678487802077,
+            "heading": -0.004824083437490689,
+            "angularVelocity": -0.7980299015185681,
+            "velocityX": 0.6534941530454559,
+            "velocityY": -0.5603571707963146,
+            "timestamp": 2.248967839736724
+        },
+        {
+            "x": 1.4936387312915242,
+            "y": 5.397060158439441,
+            "heading": -0.08394241765705696,
+            "angularVelocity": -1.4908477116686103,
+            "velocityX": 1.3134711805253199,
+            "velocityY": -1.1254086982009737,
+            "timestamp": 2.3020371999405342
+        },
+        {
+            "x": 1.5989756629525658,
+            "y": 5.306833356180067,
+            "heading": -0.18501626934212806,
+            "angularVelocity": -1.9045613381601734,
+            "velocityX": 1.9848916824416323,
+            "velocityY": -1.7001675149815771,
+            "timestamp": 2.3551065601443444
+        },
+        {
+            "x": 1.7389822594115079,
+            "y": 5.188268009617622,
+            "heading": -0.23253583212093973,
+            "angularVelocity": -0.8954236982755346,
+            "velocityX": 2.6381813521258652,
+            "velocityY": -2.234158205546495,
+            "timestamp": 2.4081759203481545
+        },
+        {
+            "x": 1.8903539316186226,
+            "y": 5.05728781912084,
+            "heading": -0.23253589895082247,
+            "angularVelocity": -0.0000012592931680036756,
+            "velocityX": 2.85233648240303,
+            "velocityY": -2.4680943955939854,
+            "timestamp": 2.4612452805519647
+        },
+        {
+            "x": 2.041725559967012,
+            "y": 4.926307577937187,
+            "heading": -0.23253596578056132,
+            "angularVelocity": -0.000001259290456718511,
+            "velocityX": 2.8523356559614563,
+            "velocityY": -2.468095350700103,
+            "timestamp": 2.514314640755775
+        },
+        {
+            "x": 2.186666927649336,
+            "y": 4.788245829195716,
+            "heading": -0.23253707199818868,
+            "angularVelocity": -0.000020844751532055486,
+            "velocityX": 2.731168552356477,
+            "velocityY": -2.601534071849594,
+            "timestamp": 2.567384000959585
+        },
+        {
+            "x": 2.299267843418341,
+            "y": 4.670864736575426,
+            "heading": -0.33873841006148575,
+            "angularVelocity": -2.0011799210587196,
+            "velocityX": 2.1217688575209404,
+            "velocityY": -2.2118429950821876,
+            "timestamp": 2.620453361163395
+        },
+        {
+            "x": 2.37528105951498,
+            "y": 4.58047733131235,
+            "heading": -0.49175658993248267,
+            "angularVelocity": -2.883362062088883,
+            "velocityX": 1.4323371490576546,
+            "velocityY": -1.7031937998865565,
+            "timestamp": 2.6735227213672053
+        },
+        {
+            "x": 2.4143476486206055,
+            "y": 4.51963472366333,
+            "heading": -0.6391760227970993,
+            "angularVelocity": -2.777863390447102,
+            "velocityX": 0.7361420781330721,
+            "velocityY": -1.146473358927959,
+            "timestamp": 2.7265920815710154
+        },
+        {
+            "x": 2.3701336666128094,
+            "y": 4.4395971700859445,
+            "heading": -0.8245153775471664,
+            "angularVelocity": -2.372308285003151,
+            "velocityX": -0.5659305114745719,
+            "velocityY": -1.0244653744427692,
+            "timestamp": 2.8047182492034923
+        },
+        {
+            "x": 2.2242072743116044,
+            "y": 4.369198833257615,
+            "heading": -0.9780363443258114,
+            "angularVelocity": -1.9650390058915468,
+            "velocityX": -1.8678299054380199,
+            "velocityY": -0.901085244056695,
+            "timestamp": 2.8828444168359693
+        },
+        {
+            "x": 1.9766118523863694,
+            "y": 4.308771886473381,
+            "heading": -1.099290083952399,
+            "angularVelocity": -1.5520246711313,
+            "velocityX": -3.169174035132216,
+            "velocityY": -0.7734533590395138,
+            "timestamp": 2.960970584468446
+        },
+        {
+            "x": 1.6829782303645726,
+            "y": 4.333642341587589,
+            "heading": -1.099290070338212,
+            "angularVelocity": 1.7425898802224296e-7,
+            "velocityX": -3.758454189166382,
+            "velocityY": 0.31833706769275627,
+            "timestamp": 3.039096752100923
+        },
+        {
+            "x": 1.3895177021759386,
+            "y": 4.36047817582874,
+            "heading": -1.0992900564673602,
+            "angularVelocity": 1.7754424736252278e-7,
+            "velocityX": -3.7562386212150933,
+            "velocityY": 0.34349354453672953,
+            "timestamp": 3.1172229197334
+        },
+        {
+            "x": 1.0960571742019716,
+            "y": 4.38731401241607,
+            "heading": -1.0992900425963372,
+            "angularVelocity": 1.7754644295782002e-7,
+            "velocityX": -3.756238618467393,
+            "velocityY": 0.34349357456735763,
+            "timestamp": 3.195349087365877
+        },
+        {
+            "x": 0.8925519822862651,
+            "y": 4.405915634394149,
+            "heading": -1.0363057213411215,
+            "angularVelocity": 0.8061872630368462,
+            "velocityX": -2.6048275255614968,
+            "velocityY": 0.23809720279105512,
+            "timestamp": 3.273475254998354
+        },
+        {
+            "x": 0.7907993793487549,
+            "y": 4.415216445922852,
+            "heading": -1.0048134836944498,
+            "angularVelocity": 0.4030946173477082,
+            "velocityX": -1.302413852119022,
+            "velocityY": 0.11904860830311666,
+            "timestamp": 3.351601422630831
+        },
+        {
+            "x": 0.7907993793487549,
+            "y": 4.415216445922852,
+            "heading": -1.0048134836944498,
+            "angularVelocity": -9.422070435540628e-32,
+            "velocityX": 1.9369819218763315e-32,
+            "velocityY": -1.162688749272308e-31,
+            "timestamp": 3.4297275902633078
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto
new file mode 100644
index 00000000..3e90ad8b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/local 4.auto	
@@ -0,0 +1,100 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7470832467079163,
+      "y": 6.625458717346191
+    },
+    "rotation": 60.53273310724105
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Fender"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "amp 4 local sgmt 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Fender"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "amp 4 local sgmt 2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Fender"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "amp 4 local sgmt 3"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Fender"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 7026fb97..bab0da93 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"height":8.02},"nodeSizeMeters":0.2,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From acdfcd94deb623e3c4024cdae68d8c7324354529 Mon Sep 17 00:00:00 2001
From: Lewy Seiden <lewy@seiden.us>
Date: Mon, 8 Jan 2024 23:20:59 -0800
Subject: [PATCH 06/10] ran sim 4 ring

---
 .pathplanner/settings.json                    | 12 ++++
 .../deploy/pathplanner/autos/New Auto.auto    | 25 -------
 .../deploy/pathplanner/autos/local 4.auto     | 14 ++--
 .../pathplanner/paths/Example Path.path       | 65 -------------------
 src/main/java/frc/robot/Robot.java            | 14 +++-
 .../subsystems/swerve/SwerveSubsystem.java    |  2 +
 6 files changed, 34 insertions(+), 98 deletions(-)
 create mode 100644 .pathplanner/settings.json
 delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 delete mode 100644 src/main/deploy/pathplanner/paths/Example Path.path

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
new file mode 100644
index 00000000..2aaab015
--- /dev/null
+++ b/.pathplanner/settings.json
@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
deleted file mode 100644
index 3a6d0044..00000000
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ /dev/null
@@ -1,25 +0,0 @@
-{
-  "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 2.0,
-      "y": 7.0
-    },
-    "rotation": 0
-  },
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Example Path"
-          }
-        }
-      ]
-    }
-  },
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto
index 3e90ad8b..67fcef43 100644
--- a/src/main/deploy/pathplanner/autos/local 4.auto	
+++ b/src/main/deploy/pathplanner/autos/local 4.auto	
@@ -14,7 +14,7 @@
         {
           "type": "named",
           "data": {
-            "name": "Fender"
+            "name": "fender"
           }
         },
         {
@@ -30,7 +30,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "Intake"
+                  "name": "intake"
                 }
               }
             ]
@@ -39,7 +39,7 @@
         {
           "type": "named",
           "data": {
-            "name": "Fender"
+            "name": "fender"
           }
         },
         {
@@ -55,7 +55,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "Intake"
+                  "name": "intake"
                 }
               }
             ]
@@ -64,7 +64,7 @@
         {
           "type": "named",
           "data": {
-            "name": "Fender"
+            "name": "fender"
           }
         },
         {
@@ -80,7 +80,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "Intake"
+                  "name": "intake"
                 }
               }
             ]
@@ -89,7 +89,7 @@
         {
           "type": "named",
           "data": {
-            "name": "Fender"
+            "name": "fender"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
deleted file mode 100644
index 5a654c84..00000000
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ /dev/null
@@ -1,65 +0,0 @@
-{
-  "version": 1.0,
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 2.0,
-        "y": 7.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.0,
-        "y": 6.5
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.0,
-        "y": 5.0
-      },
-      "prevControl": {
-        "x": 6.7282857142857155,
-        "y": 12.004571428571431
-      },
-      "nextControl": {
-        "x": 4.661221351704417,
-        "y": 3.626963573877582
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.0,
-        "y": 1.0
-      },
-      "prevControl": {
-        "x": 12.442,
-        "y": 6.521714285714287
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0,
-    "rotateFast": false
-  },
-  "reversed": false,
-  "folder": null,
-  "previewStartingState": null,
-  "useDefaultConstraints": false
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8820597b..3f0bfddf 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,12 +4,14 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.swerve.GyroIO;
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
@@ -92,6 +94,16 @@ public void robotInit() {
                     -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED,
                     -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED,
                     controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED)));
+
+    // Auto Bindings here
+    NamedCommands.registerCommand(
+        "fender",
+        Commands.deadline(
+          Commands.sequence(
+            Commands.print("fender shot"), Commands.waitSeconds(1.0), Commands.print("pew!")),
+            swerve.stopCmd()));
+    NamedCommands.registerCommand("intake", Commands.print("intake"));
+    NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
   }
 
   @Override
@@ -118,7 +130,7 @@ public void autonomousInit() {
   }
 
   public Command getAutonomousCommand() {
-    return new PathPlannerAuto("New Auto");
+    return new PathPlannerAuto("local 4");
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 98cbee12..d405b441 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -17,6 +17,7 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -91,6 +92,7 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
             ),
         this // Reference to this subsystem to set requirements
         );
+    PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("PathPlanner/Target", pose));
   }
 
   /**

From 959c0c6ceb43e2deb449495f1db961e6234b22ce Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <Lewis-Seiden@users.noreply.github.com>
Date: Tue, 9 Jan 2024 07:22:04 +0000
Subject: [PATCH 07/10] run spotless

---
 src/main/java/frc/robot/Robot.java                            | 4 ++--
 .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java     | 3 ++-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 3f0bfddf..36812685 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -99,8 +99,8 @@ public void robotInit() {
     NamedCommands.registerCommand(
         "fender",
         Commands.deadline(
-          Commands.sequence(
-            Commands.print("fender shot"), Commands.waitSeconds(1.0), Commands.print("pew!")),
+            Commands.sequence(
+                Commands.print("fender shot"), Commands.waitSeconds(1.0), Commands.print("pew!")),
             swerve.stopCmd()));
     NamedCommands.registerCommand("intake", Commands.print("intake"));
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index d405b441..cb659298 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -92,7 +92,8 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
             ),
         this // Reference to this subsystem to set requirements
         );
-    PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("PathPlanner/Target", pose));
+    PathPlannerLogging.setLogTargetPoseCallback(
+        (pose) -> Logger.recordOutput("PathPlanner/Target", pose));
   }
 
   /**

From b7d6a736db3dee32ab8c551eb06b5bbfb332a28e Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 9 Jan 2024 15:47:30 -0800
Subject: [PATCH 08/10] merged getAutonomousCommand with autonomousInit renamed
 consumeChassisSpeeds to runVelocity

---
 src/main/java/frc/robot/Robot.java                          | 6 +-----
 .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java   | 6 +++---
 2 files changed, 4 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 36812685..36b5abff 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -122,17 +122,13 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
-    autonomousCommand = getAutonomousCommand();
+    autonomousCommand = new PathPlannerAuto("local 4");
 
     if (autonomousCommand != null) {
       autonomousCommand.schedule();
     }
   }
 
-  public Command getAutonomousCommand() {
-    return new PathPlannerAuto("local 4");
-  }
-
   @Override
   public void autonomousPeriodic() {}
 
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index cb659298..051eeb52 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -79,7 +79,7 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
         this::getPose, // Robot pose supplier
         this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
         this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-        this::consumeChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE
+        this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE
         // ChassisSpeeds
         new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in
             // your Constants class
@@ -179,7 +179,7 @@ public void periodic() {
     }
   }
 
-  private void consumeChassisSpeeds(ChassisSpeeds speeds) {
+  private void runVelocity(ChassisSpeeds speeds) {
     // Calculate module setpoints
     ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
     SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
@@ -202,7 +202,7 @@ private void consumeChassisSpeeds(ChassisSpeeds speeds) {
    * @param speeds Speeds in meters/sec
    */
   public Command runVelocityCmd(Supplier<ChassisSpeeds> speeds) {
-    return this.run(() -> consumeChassisSpeeds(speeds.get()));
+    return this.run(() -> runVelocity(speeds.get()));
   }
 
   /** Stops the drive. */

From 82a51e454d4b480ece16488eb9355a473de8d795 Mon Sep 17 00:00:00 2001
From: stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 9 Jan 2024 21:44:03 -0800
Subject: [PATCH 09/10] updated pathplannerLib and added shouldFlipPath = true
 to auto config

---
 .vscode/settings.json                                       | 3 ++-
 .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java   | 2 +-
 vendordeps/PathplannerLib.json                              | 6 +++---
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 4ed293b7..2f1b9b39 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -25,5 +25,6 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.compile.nullAnalysis.mode": "automatic"
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 65460eae..9f1ab4f0 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -90,7 +90,7 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
             0.4, // Drive base radius in meters. Distance from robot center to furthest module.
             new ReplanningConfig() // Default path replanning config. See the API for the options
             // here
-            ),
+            ), () -> true,
         this // Reference to this subsystem to set requirements
         );
     PathPlannerLogging.setLogTargetPoseCallback(
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index 3e18a45d..3c741462 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.0.0-beta-6.2",
+    "version": "2024.1.2",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.0.0-beta-6.2"
+            "version": "2024.1.2"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.0.0-beta-6.2",
+            "version": "2024.1.2",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

From a9e839217dd6918be056b27eda62aca6d79cd6ba Mon Sep 17 00:00:00 2001
From: stoopipoopy <stoopipoopy@users.noreply.github.com>
Date: Wed, 10 Jan 2024 05:45:42 +0000
Subject: [PATCH 10/10] run spotless

---
 src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 9f1ab4f0..cdfce767 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -90,7 +90,8 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
             0.4, // Drive base radius in meters. Distance from robot center to furthest module.
             new ReplanningConfig() // Default path replanning config. See the API for the options
             // here
-            ), () -> true,
+            ),
+        () -> true,
         this // Reference to this subsystem to set requirements
         );
     PathPlannerLogging.setLogTargetPoseCallback(