From 1e87959dc43be9a008835d44fe48d37626656d25 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Sat, 7 Feb 2026 12:31:02 -0800
Subject: [PATCH 01/54] find and log time left in shift also the time stamps
 were wrong

---
 src/main/java/frc/robot/Superstructure.java | 42 +++++++++++++++++----
 1 file changed, 34 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index a4fcf504..dd955c7b 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -47,9 +47,14 @@ public Trigger getTrigger() {
   @AutoLogOutput(key = "Superstructure/State")
   private static SuperState state = SuperState.IDLE;
 
+  @AutoLogOutput(key = "Superstructure/Shift Timer")
+  private double timeLeftInShift = getTimeLeftInShift(getCurrentShift());
+
+  @AutoLogOutput(key = "Superstructure/Current Shift")
+  private int currentShift = getCurrentShift();
+
   @AutoLogOutput(key = "Scoring/Scoring Active")
-  public boolean isScoringActive =
-      isOurShift(); // assuming we want the dashboard to show if the time allows us to score not if
+  public boolean isScoringActive = isOurShift(); 
 
   // its litterly possible
 
@@ -85,6 +90,8 @@ public Trigger getTrigger() {
 
   private boolean shouldFeed = false;
 
+  private double timeLeftinMatch = Timer.getMatchTime();
+
   // @AutoLogOutput(key = "Superstructure/At Extension?")
   // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation);
 
@@ -365,19 +372,38 @@ private Alliance getStartingAlliance() {
   }
 
   private int getCurrentShift() {
-    double timeLeftinMatch = Timer.getMatchTime();
     // may be a nicer way to do this
-    if (105.00 <= timeLeftinMatch && timeLeftinMatch <= 130.00) {
+    if (210.00 < timeLeftinMatch && timeLeftinMatch <= 220.00) {
+      return 0; // transition shift
+      // My numbers were wrong for some reason not sure
+    } else if (145.00 < timeLeftinMatch && timeLeftinMatch <= 210.00) {
       return 1;
-    } else if (80.00 <= timeLeftinMatch && timeLeftinMatch <= 105.00) {
+    } else if (120.00 < timeLeftinMatch && timeLeftinMatch <= 145.00) {
       return 2;
-    } else if ((55.00 <= timeLeftinMatch && timeLeftinMatch <= 80.00)) {
+    } else if ((55.00 < timeLeftinMatch && timeLeftinMatch <= 120.00)) {
       return 3;
-    } else if ((30.00 <= timeLeftinMatch && timeLeftinMatch <= 55.00)) {
+    } else if ((30.00 < timeLeftinMatch && timeLeftinMatch <= 55.00)) {
       return 4;
     } else {
-      return 0;
+      return 5; // endgame or whatever
+    }
+  }
+
+  private double getTimeLeftInShift(int currentShift) {
+    if (currentShift == 0) {
+      timeLeftInShift = timeLeftinMatch - 210.00;
+    } else if (currentShift == 1) {
+      timeLeftInShift = timeLeftinMatch - 145.00;
+    } else if (currentShift == 2) {
+      timeLeftInShift = timeLeftinMatch - 120.00;
+    } else if (currentShift == 3) {
+      timeLeftInShift = timeLeftinMatch - 55.00;
+    } else if (currentShift == 4) {
+      timeLeftInShift = timeLeftinMatch - 30.00;
+    } else {
+      timeLeftInShift = timeLeftinMatch;
     }
+    return timeLeftInShift;
   }
 
   public boolean isOurShift() {

From 309243d378e3e1d7598ed13cf3070832583d6934 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 6 Mar 2026 23:02:37 -0800
Subject: [PATCH 02/54] use switch less chopped

---
 src/main/java/frc/robot/Superstructure.java | 24 +++++++++------------
 1 file changed, 10 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index dd955c7b..6741eff9 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -390,20 +390,16 @@ private int getCurrentShift() {
   }
 
   private double getTimeLeftInShift(int currentShift) {
-    if (currentShift == 0) {
-      timeLeftInShift = timeLeftinMatch - 210.00;
-    } else if (currentShift == 1) {
-      timeLeftInShift = timeLeftinMatch - 145.00;
-    } else if (currentShift == 2) {
-      timeLeftInShift = timeLeftinMatch - 120.00;
-    } else if (currentShift == 3) {
-      timeLeftInShift = timeLeftinMatch - 55.00;
-    } else if (currentShift == 4) {
-      timeLeftInShift = timeLeftinMatch - 30.00;
-    } else {
-      timeLeftInShift = timeLeftinMatch;
-    }
-    return timeLeftInShift;
+    double offset = switch(currentShift) {
+      case 0 -> 210.00;
+      case 1 -> 145.00;
+      case 2 -> 120.00;
+      case 3 -> 55.00;
+      case 4 -> 30.00;
+      default -> 0.00;
+    };
+    return timeLeftinMatch - offset;
+    
   }
 
   public boolean isOurShift() {

From de892ecbc70937c634f7aa754395febedb35fcb7 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 6 Mar 2026 23:31:07 -0800
Subject: [PATCH 03/54] im dumb

---
 src/main/java/frc/robot/Robot.java          |  4 +-
 src/main/java/frc/robot/Superstructure.java | 64 +++++++++++----------
 2 files changed, 38 insertions(+), 30 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 01d26b8a..8bb92dc3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -614,7 +614,9 @@ public void autonomousPeriodic() {}
   public void autonomousExit() {}
 
   @Override
-  public void teleopInit() {}
+  public void teleopInit() {
+    Superstructure.matchStartTime = Timer.getFPGATimestamp();
+  }
 
   @Override
   public void teleopPeriodic() {}
diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 6741eff9..2991da8f 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -48,7 +48,7 @@ public Trigger getTrigger() {
   private static SuperState state = SuperState.IDLE;
 
   @AutoLogOutput(key = "Superstructure/Shift Timer")
-  private double timeLeftInShift = getTimeLeftInShift(getCurrentShift());
+  private double timeLeftInShift = getTimeLeftInShift();
 
   @AutoLogOutput(key = "Superstructure/Current Shift")
   private int currentShift = getCurrentShift();
@@ -56,12 +56,22 @@ public Trigger getTrigger() {
   @AutoLogOutput(key = "Scoring/Scoring Active")
   public boolean isScoringActive = isOurShift(); 
 
-  // its litterly possible
-
   private SuperState prevState = SuperState.IDLE;
 
   private Timer stateTimer = new Timer();
 
+  //private double timeLeftinMatch = Timer.getMatchTime();
+
+  private double timer = Timer.getFPGATimestamp();
+
+  public static double matchStartTime;
+
+  private double timeElapsed = timer - matchStartTime;
+
+  private double timeLeftinMatch() {
+    return 140.00 - timeElapsed;
+  }
+
   private final SwerveSubsystem swerve;
   private final Indexer indexer;
   private final Intake intake;
@@ -90,8 +100,6 @@ public Trigger getTrigger() {
 
   private boolean shouldFeed = false;
 
-  private double timeLeftinMatch = Timer.getMatchTime();
-
   // @AutoLogOutput(key = "Superstructure/At Extension?")
   // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation);
 
@@ -371,43 +379,41 @@ private Alliance getStartingAlliance() {
     }
   }
 
-  private int getCurrentShift() {
-    // may be a nicer way to do this
-    if (210.00 < timeLeftinMatch && timeLeftinMatch <= 220.00) {
-      return 0; // transition shift
-      // My numbers were wrong for some reason not sure
-    } else if (145.00 < timeLeftinMatch && timeLeftinMatch <= 210.00) {
-      return 1;
-    } else if (120.00 < timeLeftinMatch && timeLeftinMatch <= 145.00) {
-      return 2;
-    } else if ((55.00 < timeLeftinMatch && timeLeftinMatch <= 120.00)) {
-      return 3;
-    } else if ((30.00 < timeLeftinMatch && timeLeftinMatch <= 55.00)) {
-      return 4;
+  private String getCurrentShift() {
+    if (130.00 < timeLeftinMatch() && timeLeftinMatch() <= 140.00) {
+      return "Transition"; 
+    } else if (105.00 < timeLeftinMatch() && timeLeftinMatch()<= 130.00) {
+      return "Shift 1";
+    } else if (80.00 < timeLeftinMatch() && timeLeftinMatch() <= 105.00) {
+      return "Shift 2";
+    } else if ((55.00 < timeLeftinMatch() && timeLeftinMatch() <= 80.00)) {
+      return "Shift 3";
+    } else if ((30.00 < timeLeftinMatch() && timeLeftinMatch() <= 55.00)) {
+      return "Shift 4";
     } else {
-      return 5; // endgame or whatever
+      return "End Game"; 
     }
   }
 
-  private double getTimeLeftInShift(int currentShift) {
-    double offset = switch(currentShift) {
-      case 0 -> 210.00;
-      case 1 -> 145.00;
-      case 2 -> 120.00;
-      case 3 -> 55.00;
-      case 4 -> 30.00;
+  private double getTimeLeftInShift() {
+    double offset = switch(getCurrentShift()) {
+      case "Transition" -> 140.00;
+      case "Shift 1" -> 130.00;
+      case "Shift 2" -> 105.00;
+      case "Shift 3" -> 80.00;
+      case "Shift 4" -> 55.00;
       default -> 0.00;
     };
-    return timeLeftinMatch - offset;
+    return timeLeftinMatch() - offset;
     
   }
 
   public boolean isOurShift() {
     // only cant score when its the others turn, otherwise everyone can
     if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) {
-      return !(getCurrentShift() == 2 || getCurrentShift() == 4);
+      return !(getCurrentShift() == "Shift 2" || getCurrentShift() == "Shift 4");
     } else {
-      return !(getCurrentShift() == 1 || getCurrentShift() == 3);
+      return !(getCurrentShift() == "Shift 1" || getCurrentShift() == "Shift 3");
     }
   }
 

From b11a3d0bdae2e585f9c03781d1aaf8dfa6ffb254 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 6 Mar 2026 23:34:00 -0800
Subject: [PATCH 04/54] i made it based on FPGA timer not fms

---
 src/main/java/frc/robot/Superstructure.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 2991da8f..dfc6e622 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -51,7 +51,7 @@ public Trigger getTrigger() {
   private double timeLeftInShift = getTimeLeftInShift();
 
   @AutoLogOutput(key = "Superstructure/Current Shift")
-  private int currentShift = getCurrentShift();
+  private String currentShift = getCurrentShift();
 
   @AutoLogOutput(key = "Scoring/Scoring Active")
   public boolean isScoringActive = isOurShift(); 

From 9962292277aa6eae7e0ec3376e0980025070a44a Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 6 Mar 2026 23:43:01 -0800
Subject: [PATCH 05/54] log shift as string

---
 src/main/java/frc/robot/Superstructure.java | 28 ++++++++++-----------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index dfc6e622..be797324 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -54,13 +54,13 @@ public Trigger getTrigger() {
   private String currentShift = getCurrentShift();
 
   @AutoLogOutput(key = "Scoring/Scoring Active")
-  public boolean isScoringActive = isOurShift(); 
+  public boolean isScoringActive = isOurShift();
 
   private SuperState prevState = SuperState.IDLE;
 
   private Timer stateTimer = new Timer();
 
-  //private double timeLeftinMatch = Timer.getMatchTime();
+  // private double timeLeftinMatch = Timer.getMatchTime();
 
   private double timer = Timer.getFPGATimestamp();
 
@@ -381,8 +381,8 @@ private Alliance getStartingAlliance() {
 
   private String getCurrentShift() {
     if (130.00 < timeLeftinMatch() && timeLeftinMatch() <= 140.00) {
-      return "Transition"; 
-    } else if (105.00 < timeLeftinMatch() && timeLeftinMatch()<= 130.00) {
+      return "Transition";
+    } else if (105.00 < timeLeftinMatch() && timeLeftinMatch() <= 130.00) {
       return "Shift 1";
     } else if (80.00 < timeLeftinMatch() && timeLeftinMatch() <= 105.00) {
       return "Shift 2";
@@ -391,21 +391,21 @@ private String getCurrentShift() {
     } else if ((30.00 < timeLeftinMatch() && timeLeftinMatch() <= 55.00)) {
       return "Shift 4";
     } else {
-      return "End Game"; 
+      return "End Game";
     }
   }
 
   private double getTimeLeftInShift() {
-    double offset = switch(getCurrentShift()) {
-      case "Transition" -> 140.00;
-      case "Shift 1" -> 130.00;
-      case "Shift 2" -> 105.00;
-      case "Shift 3" -> 80.00;
-      case "Shift 4" -> 55.00;
-      default -> 0.00;
-    };
+    double offset =
+        switch (getCurrentShift()) {
+          case "Transition" -> 140.00;
+          case "Shift 1" -> 130.00;
+          case "Shift 2" -> 105.00;
+          case "Shift 3" -> 80.00;
+          case "Shift 4" -> 55.00;
+          default -> 0.00;
+        };
     return timeLeftinMatch() - offset;
-    
   }
 
   public boolean isOurShift() {

From 3d679640aa3fc944de46e20c8d25ff4d2f1f3f6b Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Sat, 7 Mar 2026 00:12:18 -0800
Subject: [PATCH 06/54] make it actually update

---
 src/main/java/frc/robot/Superstructure.java | 40 +++++++++++++--------
 1 file changed, 26 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index c8eceefe..1e112282 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -73,29 +73,41 @@ public enum FeedTarget {
   @AutoLogOutput(key = "Superstructure/State")
   private static SuperState state = SuperState.IDLE;
 
-  @AutoLogOutput(key = "Superstructure/Shift Timer")
-  private double timeLeftInShift = getTimeLeftInShift();
-
-  @AutoLogOutput(key = "Superstructure/Current Shift")
-  private String currentShift = getCurrentShift();
-
-  @AutoLogOutput(key = "Scoring/Scoring Active")
-  public boolean isScoringActive = isOurShift();
-
   private SuperState prevState = SuperState.IDLE;
 
   private Timer stateTimer = new Timer();
 
-  // private double timeLeftinMatch = Timer.getMatchTime();
-
-  private double timer = Timer.getFPGATimestamp();
+  @AutoLogOutput(key = "Superstructure/FPGA time stamp")
+  private double getFPGATimestamp() {
+    return Timer.getFPGATimestamp();
+  }
 
+  @AutoLogOutput(key = "Superstructure/match starttime")
   public static double matchStartTime;
 
-  private double timeElapsed = timer - matchStartTime;
+  @AutoLogOutput(key = "Superstructure/Time elapsed")
+  private double getTimeElapsed() {
+    return getFPGATimestamp() - matchStartTime;
+  }
 
+  @AutoLogOutput(key = "Superstructure/Time Left in match")
   private double timeLeftinMatch() {
-    return 140.00 - timeElapsed;
+    return 140.00 - getTimeElapsed();
+  }
+
+  @AutoLogOutput(key = "Superstructure/Shift Timer")
+  private double getTimeStampLeftInShift() {
+    return getTimeLeftInShift();
+  }
+
+  @AutoLogOutput(key = "Superstructure/Current Shift")
+  private String getCurrentShiftName() {
+    return getCurrentShift();
+  }
+
+  @AutoLogOutput(key = "Scoring/Scoring Active")
+  public boolean isScoringActive() {
+    return isOurShift();
   }
 
   private final SwerveSubsystem swerve;

From 8dbea6e05660efa4e0257ce6ad52078d4056b302 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Sat, 7 Mar 2026 00:18:35 -0800
Subject: [PATCH 07/54] fix offset

---
 src/main/java/frc/robot/Superstructure.java | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 1e112282..7180c183 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -635,11 +635,11 @@ private String getCurrentShift() {
   private double getTimeLeftInShift() {
     double offset =
         switch (getCurrentShift()) {
-          case "Transition" -> 140.00;
-          case "Shift 1" -> 130.00;
-          case "Shift 2" -> 105.00;
-          case "Shift 3" -> 80.00;
-          case "Shift 4" -> 55.00;
+          case "Transition" -> 130.00;
+          case "Shift 1" -> 105.00;
+          case "Shift 2" -> 80.00;
+          case "Shift 3" -> 55.00;
+          case "Shift 4" -> 30.00;
           default -> 0.00;
         };
     return timeLeftinMatch() - offset;

From 1b50a2be6ae2b07399caaee93ea3536039583c0b Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Sat, 7 Mar 2026 00:20:31 -0800
Subject: [PATCH 08/54] remove extra logging

---
 src/main/java/frc/robot/Superstructure.java | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 7180c183..818d3c20 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -77,7 +77,6 @@ public enum FeedTarget {
 
   private Timer stateTimer = new Timer();
 
-  @AutoLogOutput(key = "Superstructure/FPGA time stamp")
   private double getFPGATimestamp() {
     return Timer.getFPGATimestamp();
   }
@@ -85,12 +84,10 @@ private double getFPGATimestamp() {
   @AutoLogOutput(key = "Superstructure/match starttime")
   public static double matchStartTime;
 
-  @AutoLogOutput(key = "Superstructure/Time elapsed")
   private double getTimeElapsed() {
     return getFPGATimestamp() - matchStartTime;
   }
 
-  @AutoLogOutput(key = "Superstructure/Time Left in match")
   private double timeLeftinMatch() {
     return 140.00 - getTimeElapsed();
   }

From 92f903d1049239d111d082637aa29196e74bc32a Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Fri, 13 Mar 2026 22:50:27 -0700
Subject: [PATCH 09/54] remove score -> spinup again (idk may revert)

---
 src/main/java/frc/robot/Superstructure.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index fedf75e7..abfa896d 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -270,7 +270,7 @@ private void addTransitions() {
 
     bindTransition(SuperState.SPIN_UP_SCORE, SuperState.SCORE, readyTrigger);
 
-    bindTransition(SuperState.SCORE, SuperState.SPIN_UP_SCORE, readyTrigger.negate());
+    // bindTransition(SuperState.SCORE, SuperState.SPIN_UP_SCORE, readyTrigger.negate());
 
     bindTransition(SuperState.SPIN_UP_SCORE, SuperState.IDLE, shootReq.negate());
 

From 8e870faa8bfcda7be18274500c251fe98bf909b1 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 13 Mar 2026 23:31:27 -0700
Subject: [PATCH 10/54] add simple scoring path in the center and outpost only
 path and rename some pose variables for clarity

---
 src/main/deploy/choreo/CtoO.traj          | 115 ----------------------
 src/main/deploy/choreo/DtoC.traj          | 113 ---------------------
 src/main/deploy/choreo/DtoS.traj          | 112 +++++++++++++++++++++
 src/main/deploy/choreo/NewPath.traj       |  28 ++++++
 src/main/deploy/choreo/OtoS.traj          | 104 +++++++++++++++++++
 src/main/deploy/choreo/PMtoM.traj         |  80 +++++++++++++++
 src/main/deploy/choreo/StoO.traj          | 115 ++++++++++++++++++++++
 src/main/deploy/choreo/rebuiltChoreo.chor |  56 ++++++++---
 src/main/java/frc/robot/Autos.java        |  39 +++++++-
 9 files changed, 517 insertions(+), 245 deletions(-)
 delete mode 100644 src/main/deploy/choreo/CtoO.traj
 delete mode 100644 src/main/deploy/choreo/DtoC.traj
 create mode 100644 src/main/deploy/choreo/DtoS.traj
 create mode 100644 src/main/deploy/choreo/NewPath.traj
 create mode 100644 src/main/deploy/choreo/OtoS.traj
 create mode 100644 src/main/deploy/choreo/PMtoM.traj
 create mode 100644 src/main/deploy/choreo/StoO.traj

diff --git a/src/main/deploy/choreo/CtoO.traj b/src/main/deploy/choreo/CtoO.traj
deleted file mode 100644
index 1bcfebbe..00000000
--- a/src/main/deploy/choreo/CtoO.traj
+++ /dev/null
@@ -1,115 +0,0 @@
-{
- "name":"CtoO",
- "version":3,
- "snapshot":{
-  "waypoints":[
-    {"x":2.4044814109802246, "y":2.0, "heading":-3.120762420732352, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.6131027936935425, "y":1.1535886526107788, "heading":1.9909931005444836, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
-    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}],
-  "targetDt":0.05
- },
- "params":{
-  "waypoints":[
-    {"x":{"exp":"CC.x", "val":2.4044814109802246}, "y":{"exp":"CC.y", "val":2.0}, "heading":{"exp":"CC.heading", "val":-3.120762420732352}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.6131027936935425 m", "val":1.6131027936935425}, "y":{"exp":"1.1535886526107788 m", "val":1.1535886526107788}, "heading":{"exp":"1.9909931005444836 rad", "val":1.9909931005444836}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
-    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}],
-  "targetDt":{
-   "exp":"0.05 s",
-   "val":0.05
-  }
- },
- "trajectory":{
-  "config":{
-   "frontLeft":{
-    "x":0.2761996,
-    "y":0.2761996
-   },
-   "backLeft":{
-    "x":-0.2761996,
-    "y":0.2761996
-   },
-   "mass":66.133767546,
-   "inertia":10.42384825260472,
-   "gearing":7.03,
-   "radius":0.0508,
-   "vmax":485.8996637552213,
-   "tmax":0.9,
-   "cof":2.25,
-   "bumper":{
-    "front":0.44196,
-    "side":0.44196,
-    "back":0.44196
-   },
-   "differentialTrackWidth":0.5427218
-  },
-  "sampleType":"Swerve",
-  "waypoints":[0.0,0.8731,1.62816],
-  "samples":[
-    {"t":0.0, "x":2.40448, "y":2.0, "heading":-3.12076, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.10375, "ay":-5.52486, "alpha":-0.03558, "fx":[-84.73034,-84.38983,-84.03414,-84.37577], "fy":[-91.02307,-91.33985,-91.66623,-91.35086]},
-    {"t":0.0291, "x":2.40232, "y":1.99766, "heading":-3.12076, "vx":-0.14854, "vy":-0.16079, "omega":-0.00104, "ax":-5.10219, "ay":-5.52318, "alpha":-0.04015, "fx":[-84.74923,-84.36524,-83.96374,-84.34916], "fy":[-90.95391,-91.31156,-91.67958,-91.32373]},
-    {"t":0.05821, "x":2.39584, "y":1.99064, "heading":-3.12079, "vx":-0.29703, "vy":-0.32153, "omega":-0.0022, "ax":-5.09988, "ay":-5.52067, "alpha":-0.04698, "fx":[-84.77733,-84.32862,-83.85875,-84.30941], "fy":[-90.85077,-91.26939,-91.69941,-91.28324]},
-    {"t":0.08731, "x":2.38503, "y":1.97895, "heading":-3.12086, "vx":-0.44545, "vy":-0.4822, "omega":-0.00357, "ax":-5.09605, "ay":-5.51652, "alpha":-0.05826, "fx":[-84.82358,-84.2683,-83.68536,-84.24362], "fy":[-90.68041,-91.19989,-91.732,-91.21627]},
-    {"t":0.11641, "x":2.36991, "y":1.96258, "heading":-3.12096, "vx":-0.59376, "vy":-0.64275, "omega":-0.00527, "ax":-5.08851, "ay":-5.50836, "alpha":-0.08044, "fx":[-84.91387,-84.15016,-83.34448,-84.11384], "fy":[-90.34539,-91.06376,-91.79541,-91.08419]},
-    {"t":0.14552, "x":2.35047, "y":1.94154, "heading":-3.12111, "vx":-0.74185, "vy":-0.80307, "omega":-0.00761, "ax":-5.06683, "ay":-5.48489, "alpha":-0.14418, "fx":[-85.16818,-83.81475,-82.36729,-83.73853], "fy":[-89.38428,-90.67721,-91.97258,-90.70222]},
-    {"t":0.17462, "x":2.32674, "y":1.91584, "heading":-3.12134, "vx":-0.88932, "vy":-0.96269, "omega":-0.0118, "ax":-4.40317, "ay":-4.76664, "alpha":-2.019, "fx":[-89.58917,-75.90525,-55.09687,-70.60664], "fy":[-62.14649,-81.58975,-94.22461,-77.27471]},
-    {"t":0.20372, "x":2.29899, "y":1.88581, "heading":-3.12168, "vx":-1.01746, "vy":-1.10142, "omega":-0.07056, "ax":-0.00092, "ay":-0.0009, "alpha":-9.93014, "fx":[-47.7886,-45.91487,47.75001,45.89247], "fy":[45.88426,-47.77975,-45.92308,47.75885]},
-    {"t":0.23283, "x":2.26938, "y":1.85375, "heading":-3.12373, "vx":-1.01749, "vy":-1.10145, "omega":-0.35956, "ax":-0.00001, "ay":0.00001, "alpha":-9.39585, "fx":[-45.10991,-43.52674,45.10971,43.52642], "fy":[43.52667,-45.10966,-43.52649,45.10996]},
-    {"t":0.26193, "x":2.23977, "y":1.82169, "heading":-3.1342, "vx":-1.01749, "vy":-1.10145, "omega":-0.63301, "ax":0.0, "ay":0.0, "alpha":-8.828, "fx":[-41.95328,-41.33729,41.95325,41.33725], "fy":[41.33728,-41.95324,-41.33725,41.95329]},
-    {"t":0.29103, "x":2.21015, "y":1.78964, "heading":3.13057, "vx":-1.01749, "vy":-1.10145, "omega":-0.88994, "ax":0.0, "ay":0.0, "alpha":-8.2263, "fx":[-38.37755,-39.23341,38.37757,39.23343], "fy":[39.23342,-38.37756,-39.23342,38.37755]},
-    {"t":0.32014, "x":2.18054, "y":1.75758, "heading":3.10467, "vx":-1.01749, "vy":-1.10145, "omega":-1.12935, "ax":0.0, "ay":0.0, "alpha":-7.59122, "fx":[-34.46525,-37.10952,34.4653,37.10957], "fy":[37.10953,-34.4653,-37.10956,34.46525]},
-    {"t":0.34924, "x":2.15093, "y":1.72553, "heading":3.0718, "vx":-1.01749, "vy":-1.10145, "omega":-1.35028, "ax":0.0, "ay":0.0, "alpha":-6.92408, "fx":[-30.30704,-34.86299,30.30712,34.8631], "fy":[34.86301,-30.30713,-34.86308,30.30703]},
-    {"t":0.37834, "x":2.12132, "y":1.69347, "heading":3.0325, "vx":-1.01749, "vy":-1.10145, "omega":-1.55179, "ax":0.0, "ay":0.0, "alpha":-6.22711, "fx":[-26.00346,-32.4003,26.00362,32.40048], "fy":[32.40033,-26.00362,-32.40045,26.00346]},
-    {"t":0.40745, "x":2.09171, "y":1.66142, "heading":2.98734, "vx":-1.01749, "vy":-1.10145, "omega":-1.73302, "ax":0.00001, "ay":-0.00001, "alpha":-5.50333, "fx":[-21.66484,-29.64271,21.66507,29.64296], "fy":[29.64274,-21.66508,-29.64293,21.66484]},
-    {"t":0.43655, "x":2.06209, "y":1.62936, "heading":2.9369, "vx":-1.01749, "vy":-1.10145, "omega":-1.89319, "ax":0.00001, "ay":-0.00001, "alpha":-4.75643, "fx":[-17.40902,-26.53101,17.40932,26.53132], "fy":[26.53104,-17.40931,-26.53128,17.40902]},
-    {"t":0.46565, "x":2.03248, "y":1.5973, "heading":2.8818, "vx":-1.01749, "vy":-1.10145, "omega":-2.03162, "ax":0.00001, "ay":-0.00001, "alpha":-3.99041, "fx":[-13.35734,-23.02873,13.35766,23.02905], "fy":[23.02876,-13.35765,-23.02902,13.35735]},
-    {"t":0.49476, "x":2.00287, "y":1.56525, "heading":2.82268, "vx":-1.01749, "vy":-1.10145, "omega":-2.14775, "ax":0.00001, "ay":-0.00001, "alpha":-3.20932, "fx":[-9.62953,-19.12352,9.62979,19.12379], "fy":[19.12354,-9.62978,-19.12377,9.62953]},
-    {"t":0.52386, "x":1.97326, "y":1.53319, "heading":2.76017, "vx":-1.01749, "vy":-1.10145, "omega":-2.24115, "ax":0.0, "ay":0.0, "alpha":-2.41694, "fx":[-6.33822,-14.82681,6.33834,14.82694], "fy":[14.82682,-6.33834,-14.82693,6.33823]},
-    {"t":0.55296, "x":1.94364, "y":1.50114, "heading":2.69494, "vx":-1.01749, "vy":-1.10145, "omega":-2.31149, "ax":0.0, "ay":0.0, "alpha":-1.61655, "fx":[-3.58398,-10.17214,3.5839,10.17206], "fy":[10.17214,-3.5839,-10.17206,3.58398]},
-    {"t":0.58207, "x":1.91403, "y":1.46908, "heading":2.62767, "vx":-1.01749, "vy":-1.10145, "omega":-2.35854, "ax":-0.00001, "ay":0.00001, "alpha":-0.811, "fx":[-1.45105,-5.21266,1.45075,5.21237], "fy":[5.21265,-1.45076,-5.21238,1.45104]},
-    {"t":0.61117, "x":1.88442, "y":1.43703, "heading":2.55903, "vx":-1.01749, "vy":-1.10145, "omega":-2.38214, "ax":-0.00001, "ay":0.00001, "alpha":-0.00275, "fx":[-0.00394,-0.01822,0.00346,0.01774], "fy":[0.0182,-0.00348,-0.01776,0.00392]},
-    {"t":0.64027, "x":1.85481, "y":1.40497, "heading":2.4897, "vx":-1.01749, "vy":-1.10145, "omega":-2.38222, "ax":-0.00002, "ay":0.00002, "alpha":0.80586, "fx":[0.71533,5.32819,-0.71598,-5.32884], "fy":[-5.32822,0.71595,5.32882,-0.71535]},
-    {"t":0.66938, "x":1.82519, "y":1.37291, "heading":2.42037, "vx":-1.01749, "vy":-1.10145, "omega":-2.35877, "ax":-0.00005, "ay":0.00005, "alpha":1.61239, "fx":[0.68904,10.73421,-0.69074,-10.73588], "fy":[-10.73428,0.69067,10.73581,-0.68911]},
-    {"t":0.69848, "x":1.79558, "y":1.34086, "heading":2.35172, "vx":-1.01749, "vy":-1.10144, "omega":-2.31185, "ax":-0.00036, "ay":0.00033, "alpha":2.41409, "fx":[-0.07801,16.09992,0.06602,-16.11151], "fy":[-16.10036,-0.06648,16.11106,0.07755]},
-    {"t":0.72758, "x":1.76597, "y":1.3088, "heading":2.28444, "vx":-1.0175, "vy":-1.10143, "omega":-2.24159, "ax":-0.00321, "ay":0.00297, "alpha":3.20784, "fx":[-1.58916,21.29502,1.47949,-21.39765], "fy":[-21.29852,-1.48407,21.39416,1.58455]},
-    {"t":0.75669, "x":1.73636, "y":1.27675, "heading":2.2192, "vx":-1.0176, "vy":-1.10135, "omega":-2.14823, "ax":-0.02884, "ay":0.02666, "alpha":3.98984, "fx":[-4.13902,25.91879,3.1327,-26.81946], "fy":[-25.94203,-3.18008,26.79669,4.08871]},
-    {"t":0.78579, "x":1.70673, "y":1.24471, "heading":2.15668, "vx":-1.01843, "vy":-1.10057, "omega":-2.03211, "ax":-0.25007, "ay":0.23296, "alpha":4.73846, "fx":[-10.80687,27.18174,1.85449,-34.76771], "fy":[-27.27189,-2.2223,34.72278,10.17796]},
-    {"t":0.81489, "x":1.67698, "y":1.21278, "heading":2.09754, "vx":-1.02571, "vy":-1.09379, "omega":-1.89421, "ax":-1.8812, "ay":1.85723, "alpha":4.37691, "fx":[-44.21365,-1.35339,-22.39854,-56.44534], "fy":[0.69301,27.90662,58.61337,35.61276]},
-    {"t":0.844, "x":1.64633, "y":1.18173, "heading":2.04241, "vx":-1.08046, "vy":-1.03974, "omega":-1.76682, "ax":-4.21606, "ay":5.00446, "alpha":0.9621, "fx":[-76.74692,-63.98311,-63.25343,-74.84019], "fy":[75.59641,85.91488,89.18704,80.26517]},
-    {"t":0.8731, "x":1.6131, "y":1.15359, "heading":1.99099, "vx":-1.20316, "vy":-0.89409, "omega":-1.73882, "ax":-3.50554, "ay":5.46389, "alpha":1.05281, "fx":[-66.08172,-50.6064,-50.71966,-64.4266], "fy":[83.75735,93.39132,96.20757,87.99106]},
-    {"t":0.90593, "x":1.57172, "y":1.12718, "heading":1.93391, "vx":-1.31825, "vy":-0.71472, "omega":-1.70426, "ax":-0.8817, "ay":1.71119, "alpha":5.29803, "fx":[-33.95829,20.84116,-0.31155,-44.88175], "fy":[-4.12933,16.96642,59.15273,41.17759]},
-    {"t":0.93876, "x":1.52796, "y":1.10464, "heading":1.87796, "vx":-1.34719, "vy":-0.65855, "omega":-1.53033, "ax":-0.08075, "ay":0.16603, "alpha":5.52672, "fx":[-18.59079,31.67083,15.3925,-33.81269], "fy":[-30.02715,-14.15798,35.43085,19.73458]},
-    {"t":0.97159, "x":1.48369, "y":1.08311, "heading":1.82772, "vx":-1.34984, "vy":-0.65309, "omega":-1.3489, "ax":-0.00727, "ay":0.01502, "alpha":5.11758, "fx":[-17.35612,29.3853,17.07449,-29.58415], "fy":[-29.23758,-16.96527,29.73171,17.46466]},
-    {"t":1.00442, "x":1.43938, "y":1.06168, "heading":1.78344, "vx":-1.35008, "vy":-0.6526, "omega":-1.18089, "ax":-0.00064, "ay":0.00133, "alpha":4.71928, "fx":[-17.07554,26.45141,17.0512,-26.46952], "fy":[-26.43848,-17.04146,26.48245,17.08528]},
-    {"t":1.03724, "x":1.39505, "y":1.04025, "heading":1.74467, "vx":-1.3501, "vy":-0.65256, "omega":-1.02597, "ax":-0.00006, "ay":0.00011, "alpha":4.33784, "fx":[-16.61612,23.69484,16.61406,-23.69645], "fy":[-23.69374,-16.61321,23.69755,16.61698]},
-    {"t":1.07007, "x":1.35073, "y":1.01883, "heading":1.71099, "vx":-1.3501, "vy":-0.65255, "omega":-0.88356, "ax":0.0, "ay":0.00001, "alpha":3.97319, "fx":[-15.9407,21.17887,15.94053,-21.179], "fy":[-21.17878,-15.94046,21.17909,15.94077]},
-    {"t":1.1029, "x":1.30641, "y":0.99741, "heading":1.68198, "vx":-1.3501, "vy":-0.65255, "omega":-0.75312, "ax":0.0, "ay":0.0, "alpha":3.62501, "fx":[-15.09798,18.89302,15.09797,-18.89303], "fy":[-18.89302,-15.09797,18.89304,15.09799]},
-    {"t":1.13573, "x":1.26209, "y":0.97599, "heading":1.65726, "vx":-1.3501, "vy":-0.65255, "omega":-0.63412, "ax":0.0, "ay":0.0, "alpha":3.2928, "fx":[-14.13441,16.81731,14.13441,-16.81731], "fy":[-16.81731,-14.13441,16.81731,14.13441]},
-    {"t":1.16856, "x":1.21777, "y":0.95456, "heading":1.63644, "vx":-1.3501, "vy":-0.65255, "omega":-0.52602, "ax":0.0, "ay":0.0, "alpha":2.97591, "fx":[-13.08777,14.92968,13.08777,-14.92968], "fy":[-14.92968,-13.08778,14.92967,13.08777]},
-    {"t":1.20139, "x":1.17344, "y":0.93314, "heading":1.61917, "vx":-1.3501, "vy":-0.65255, "omega":-0.42833, "ax":0.0, "ay":0.0, "alpha":2.67355, "fx":[-11.98787,13.20774,11.98787,-13.20774], "fy":[-13.20774,-11.98787,13.20774,11.98787]},
-    {"t":1.23422, "x":1.12912, "y":0.91172, "heading":1.60511, "vx":-1.3501, "vy":-0.65255, "omega":-0.34056, "ax":0.0, "ay":0.0, "alpha":2.3848, "fx":[-10.85774,11.62974,10.85774,-11.62974], "fy":[-11.62974,-10.85774,11.62974,10.85774]},
-    {"t":1.26705, "x":1.0848, "y":0.8903, "heading":1.59393, "vx":-1.3501, "vy":-0.65255, "omega":-0.26227, "ax":0.0, "ay":0.0, "alpha":2.10864, "fx":[-9.71477,10.17503,9.71477,-10.17503], "fy":[-10.17503,-9.71477,10.17503,9.71477]},
-    {"t":1.29987, "x":1.04048, "y":0.86887, "heading":1.58532, "vx":-1.3501, "vy":-0.65255, "omega":-0.19304, "ax":0.0, "ay":0.0, "alpha":1.84396, "fx":[-8.57169,8.82441,8.57169,-8.82441], "fy":[-8.82441,-8.57169,8.82441,8.57169]},
-    {"t":1.3327, "x":0.99615, "y":0.84745, "heading":1.57899, "vx":-1.3501, "vy":-0.65255, "omega":-0.13251, "ax":0.0, "ay":0.0, "alpha":1.58962, "fx":[-7.43741,7.56023,7.43741,-7.56023], "fy":[-7.56023,-7.43741,7.56023,7.43741]},
-    {"t":1.36553, "x":0.95183, "y":0.82603, "heading":1.57464, "vx":-1.3501, "vy":-0.65255, "omega":-0.08032, "ax":0.00002, "ay":0.00001, "alpha":1.34438, "fx":[-6.31748,6.36671,6.318,-6.36618], "fy":[-6.36631,-6.31761,6.36657,6.31787]},
-    {"t":1.39836, "x":0.90751, "y":0.80461, "heading":1.572, "vx":-1.3501, "vy":-0.65255, "omega":-0.03619, "ax":0.55769, "ay":0.26955, "alpha":1.09217, "fx":[4.10854,14.39895,14.30332,4.07139], "fy":[-0.72815,-0.70676,9.57842,9.68299]},
-    {"t":1.43119, "x":0.86349, "y":0.78333, "heading":1.57081, "vx":-1.33179, "vy":-0.6437, "omega":-0.00033, "ax":6.73048, "ay":3.25308, "alpha":0.00674, "fx":[111.29024,111.3149,111.26585,111.24111], "fy":[53.75794,53.70888,53.81138,53.8605]},
-    {"t":1.46402, "x":0.82339, "y":0.76395, "heading":1.5708, "vx":-1.11084, "vy":-0.53691, "omega":-0.00011, "ax":6.75663, "ay":3.26572, "alpha":0.00245, "fx":[111.71491,111.72378,111.70579,111.69691], "fy":[53.98392,53.96593,54.00334,54.02134]},
-    {"t":1.49685, "x":0.79057, "y":0.74808, "heading":1.5708, "vx":-0.88903, "vy":-0.4297, "omega":-0.00003, "ax":6.76526, "ay":3.26989, "alpha":0.00104, "fx":[111.85498,111.85873,111.85107,111.84732], "fy":[54.05847,54.05082,54.06671,54.07437]},
-    {"t":1.52968, "x":0.76503, "y":0.73574, "heading":1.5708, "vx":-0.66693, "vy":-0.32235, "omega":0.0, "ax":6.76956, "ay":3.27197, "alpha":0.00034, "fx":[111.92471,111.92593,111.92343,111.92221], "fy":[54.09559,54.09309,54.09827,54.10077]},
-    {"t":1.5625, "x":0.74678, "y":0.72692, "heading":1.5708, "vx":-0.4447, "vy":-0.21494, "omega":0.00001, "ax":6.77213, "ay":3.27321, "alpha":-0.00008, "fx":[111.96645,111.96616,111.96675,111.96704], "fy":[54.11781,54.1184,54.11717,54.11657]},
-    {"t":1.59533, "x":0.73583, "y":0.72163, "heading":1.5708, "vx":-0.22238, "vy":-0.10748, "omega":0.00001, "ax":6.77384, "ay":3.27404, "alpha":-0.00036, "fx":[111.99423,111.99293,111.99559,111.99688], "fy":[54.13259,54.13525,54.12975,54.12709]},
-    {"t":1.62816, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
-  "splits":[0]
- },
- "events":[]
-}
diff --git a/src/main/deploy/choreo/DtoC.traj b/src/main/deploy/choreo/DtoC.traj
deleted file mode 100644
index 950d1b0d..00000000
--- a/src/main/deploy/choreo/DtoC.traj
+++ /dev/null
@@ -1,113 +0,0 @@
-{
- "name":"DtoC",
- "version":3,
- "snapshot":{
-  "waypoints":[
-    {"x":0.703999767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":2.1980347633361816, "y":5.884655952453613, "heading":3.141666845264474, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":2.4044814109802246, "y":2.0, "heading":-3.120762420732352, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
-    {"from":2, "to":0, "data":{"type":"MaxVelocity", "props":{"max":1.8}}, "enabled":true}],
-  "targetDt":0.05
- },
- "params":{
-  "waypoints":[
-    {"x":{"exp":"D.x", "val":0.703999767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"2.1980347633361816 m", "val":2.1980347633361816}, "y":{"exp":"5.884655952453613 m", "val":5.884655952453613}, "heading":{"exp":"3.1416668452644734 rad", "val":3.141666845264474}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"CC.x", "val":2.4044814109802246}, "y":{"exp":"CC.y", "val":2.0}, "heading":{"exp":"CC.heading", "val":-3.120762420732352}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
-    {"from":2, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.8 m / s", "val":1.8}}}, "enabled":true}],
-  "targetDt":{
-   "exp":"0.05 s",
-   "val":0.05
-  }
- },
- "trajectory":{
-  "config":{
-   "frontLeft":{
-    "x":0.2761996,
-    "y":0.2761996
-   },
-   "backLeft":{
-    "x":-0.2761996,
-    "y":0.2761996
-   },
-   "mass":66.133767546,
-   "inertia":10.42384825260472,
-   "gearing":7.03,
-   "radius":0.0508,
-   "vmax":485.8996637552213,
-   "tmax":0.9,
-   "cof":2.25,
-   "bumper":{
-    "front":0.44196,
-    "side":0.44196,
-    "back":0.44196
-   },
-   "differentialTrackWidth":0.5427218
-  },
-  "sampleType":"Swerve",
-  "waypoints":[0.0,0.96944,3.27198],
-  "samples":[
-    {"t":0.0, "x":0.704, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.52414, "ay":0.20178, "alpha":-0.00003, "fx":[124.4,124.40002,124.40002,124.4], "fy":[3.33647,3.33593,3.33592,3.33646]},
-    {"t":0.05386, "x":0.71491, "y":5.97554, "heading":3.14159, "vx":0.40523, "vy":0.01087, "omega":0.0, "ax":7.52208, "ay":0.20173, "alpha":-0.00003, "fx":[124.36587,124.36589,124.36589,124.36587], "fy":[3.3356,3.33498,3.33496,3.33558]},
-    {"t":0.10772, "x":0.74765, "y":5.97642, "heading":3.14159, "vx":0.81035, "vy":0.02173, "omega":0.0, "ax":7.51796, "ay":0.20162, "alpha":-0.00004, "fx":[124.29768,124.2977,124.2977,124.29768], "fy":[3.33386,3.33307,3.33305,3.33384]},
-    {"t":0.16157, "x":0.80219, "y":5.97788, "heading":3.14159, "vx":1.21525, "vy":0.03259, "omega":-0.00001, "ax":7.50562, "ay":0.20129, "alpha":-0.00007, "fx":[124.09366,124.09369,124.0937,124.09366], "fy":[3.32864,3.32736,3.32733,3.3286]},
-    {"t":0.21543, "x":0.87853, "y":5.97993, "heading":3.14159, "vx":1.61948, "vy":0.04343, "omega":-0.00001, "ax":3.33348, "ay":0.0894, "alpha":-0.0107, "fx":[55.07276,55.07382,55.155,55.15394], "fy":[1.53896,1.41823,1.41721,1.53785]},
-    {"t":0.26929, "x":0.97059, "y":5.9824, "heading":3.14159, "vx":1.79902, "vy":0.04825, "omega":-0.00059, "ax":0.00002, "ay":0.0, "alpha":-0.01144, "fx":[-0.05362,-0.05362,0.05436,0.05436], "fy":[0.054,-0.05398,-0.05398,0.054]},
-    {"t":0.32315, "x":1.06748, "y":5.985, "heading":3.14156, "vx":1.79902, "vy":0.04825, "omega":-0.0012, "ax":0.0, "ay":0.0, "alpha":-0.00724, "fx":[-0.03416,-0.03416,0.03416,0.03416], "fy":[0.03416,-0.03416,-0.03416,0.03416]},
-    {"t":0.377, "x":1.16437, "y":5.98759, "heading":3.1415, "vx":1.79902, "vy":0.04825, "omega":-0.00159, "ax":0.0, "ay":0.0, "alpha":-0.0032, "fx":[-0.01511,-0.01512,0.01511,0.01512], "fy":[0.01512,-0.01511,-0.01512,0.01511]},
-    {"t":0.43086, "x":1.26126, "y":5.99019, "heading":3.14141, "vx":1.79902, "vy":0.04825, "omega":-0.00176, "ax":0.0, "ay":0.0, "alpha":0.00076, "fx":[0.00358,0.00358,-0.00358,-0.00358], "fy":[-0.00358,0.00358,0.00358,-0.00358]},
-    {"t":0.48472, "x":1.35815, "y":5.99279, "heading":3.14131, "vx":1.79902, "vy":0.04825, "omega":-0.00172, "ax":0.0, "ay":0.0, "alpha":0.00474, "fx":[0.02235,0.02237,-0.02235,-0.02237], "fy":[-0.02237,0.02235,0.02237,-0.02236]},
-    {"t":0.53858, "x":1.45504, "y":5.99539, "heading":3.14122, "vx":1.79902, "vy":0.04825, "omega":-0.00147, "ax":0.0, "ay":0.0, "alpha":0.00883, "fx":[0.04164,0.04167,-0.04164,-0.04167], "fy":[-0.04172,0.04159,0.04162,-0.04169]},
-    {"t":0.59243, "x":1.55193, "y":5.99799, "heading":3.14114, "vx":1.79902, "vy":0.04825, "omega":-0.00099, "ax":0.0, "ay":-0.00011, "alpha":0.01312, "fx":[0.06193,0.06198,-0.06183,-0.06189], "fy":[-0.06376,0.06006,0.06011,-0.0637]},
-    {"t":0.64629, "x":1.64882, "y":6.00059, "heading":3.14109, "vx":1.79902, "vy":0.04824, "omega":-0.00029, "ax":0.00011, "ay":-0.00399, "alpha":0.01772, "fx":[0.08531,0.08539,-0.08178,-0.08186], "fy":[-0.14958,0.01759,0.01768,-0.14949]},
-    {"t":0.70015, "x":1.74571, "y":6.00318, "heading":3.14107, "vx":1.79902, "vy":0.04803, "omega":0.00067, "ax":0.00354, "ay":-0.14434, "alpha":0.0227, "fx":[0.16561,0.16573,-0.04858,-0.04869], "fy":[-2.49346,-2.27937,-2.27926,-2.49335]},
-    {"t":0.75401, "x":1.84261, "y":6.00556, "heading":3.14111, "vx":1.79922, "vy":0.04025, "omega":0.00189, "ax":-0.13685, "ay":-3.85777, "alpha":0.0165, "fx":[-2.16307,-2.16575,-2.36229,-2.35937], "fy":[-63.8412,-63.72633,-63.72337,-63.83825]},
-    {"t":0.80786, "x":1.93931, "y":6.00213, "heading":3.14121, "vx":1.79185, "vy":-0.16752, "omega":0.00278, "ax":-1.48163, "ay":-7.18614, "alpha":0.0009, "fx":[-24.48698,-24.49016,-24.50603,-24.50286], "fy":[-118.81369,-118.8126,-118.80942,-118.81051]},
-    {"t":0.86172, "x":2.03367, "y":5.98268, "heading":3.14136, "vx":1.71205, "vy":-0.55455, "omega":0.00283, "ax":-3.08851, "ay":-6.81194, "alpha":0.00029, "fx":[-51.06039,-51.06239,-51.06685,-51.06485], "fy":[-112.62625,-112.62531,-112.6233,-112.62425]},
-    {"t":0.91558, "x":2.12139, "y":5.94294, "heading":3.14151, "vx":1.54571, "vy":-0.92142, "omega":0.00284, "ax":-4.55497, "ay":-5.96877, "alpha":0.00015, "fx":[-75.30776,-75.30913,-75.31093,-75.30956], "fy":[-98.68557,-98.68452,-98.68315,-98.6842]},
-    {"t":0.96944, "x":2.19803, "y":5.88466, "heading":-3.14152, "vx":1.30039, "vy":-1.24288, "omega":0.00285, "ax":-5.91853, "ay":-4.62574, "alpha":0.00015, "fx":[-97.85247,-97.85382,-97.85488,-97.85353], "fy":[-76.48089,-76.47916,-76.47782,-76.47955]},
-    {"t":1.03921, "x":2.27436, "y":5.78668, "heading":-3.14132, "vx":0.88743, "vy":-1.56564, "omega":0.00286, "ax":-6.9568, "ay":-2.70318, "alpha":0.00042, "fx":[-115.01808,-115.02069,-115.02177,-115.01917], "fy":[-44.69764,-44.69086,-44.68826,-44.69503]},
-    {"t":1.10898, "x":2.31935, "y":5.67085, "heading":-3.14112, "vx":0.40203, "vy":-1.75425, "omega":0.00289, "ax":-5.12946, "ay":-0.64423, "alpha":0.01353, "fx":[-84.76543,-84.78035,-84.84974,-84.83486], "fy":[-10.75175,-10.56566,-10.55101,-10.73684]},
-    {"t":1.17876, "x":2.33491, "y":5.54689, "heading":-3.14092, "vx":0.04412, "vy":-1.7992, "omega":0.00384, "ax":-0.1272, "ay":-0.00281, "alpha":0.03082, "fx":[-1.95763,-1.95783,-2.24852,-2.24832], "fy":[-0.19178,0.09913,0.09893,-0.19196]},
-    {"t":1.24853, "x":2.33768, "y":5.42134, "heading":-3.14065, "vx":0.03525, "vy":-1.7994, "omega":0.00599, "ax":-0.00167, "ay":-0.00003, "alpha":0.02524, "fx":[0.09166,0.09144,-0.14672,-0.14649], "fy":[-0.1195,0.11865,0.11843,-0.11973]},
-    {"t":1.31831, "x":2.34014, "y":5.29579, "heading":-3.14023, "vx":0.03513, "vy":-1.7994, "omega":0.00775, "ax":-0.00002, "ay":0.0, "alpha":0.02062, "fx":[0.09703,0.09677,-0.09776,-0.09749], "fy":[-0.09714,0.09739,0.09712,-0.0974]},
-    {"t":1.38808, "x":2.34259, "y":5.17024, "heading":-3.13969, "vx":0.03513, "vy":-1.7994, "omega":0.00919, "ax":0.0, "ay":0.0, "alpha":0.01679, "fx":[0.07935,0.07905,-0.07936,-0.07906], "fy":[-0.07906,0.07936,0.07906,-0.07936]},
-    {"t":1.45785, "x":2.34504, "y":5.04469, "heading":-3.13905, "vx":0.03513, "vy":-1.7994, "omega":0.01036, "ax":0.0, "ay":0.0, "alpha":0.01361, "fx":[0.06437,0.06405,-0.06437,-0.06405], "fy":[-0.06405,0.06437,0.06405,-0.06437]},
-    {"t":1.52763, "x":2.34749, "y":4.91914, "heading":-3.13833, "vx":0.03513, "vy":-1.7994, "omega":0.01131, "ax":0.0, "ay":0.0, "alpha":0.01096, "fx":[0.05186,0.05152,-0.05186,-0.05152], "fy":[-0.05152,0.05186,0.05152,-0.05186]},
-    {"t":1.5974, "x":2.34994, "y":4.79359, "heading":-3.13754, "vx":0.03513, "vy":-1.7994, "omega":0.01207, "ax":0.0, "ay":0.0, "alpha":0.00873, "fx":[0.04134,0.04101,-0.04134,-0.04101], "fy":[-0.04101,0.04134,0.04101,-0.04134]},
-    {"t":1.66717, "x":2.35239, "y":4.66803, "heading":-3.1367, "vx":0.03513, "vy":-1.7994, "omega":0.01268, "ax":0.0, "ay":0.0, "alpha":0.00683, "fx":[0.0324,0.03209,-0.0324,-0.03209], "fy":[-0.03209,0.0324,0.03209,-0.0324]},
-    {"t":1.73695, "x":2.35484, "y":4.54248, "heading":-3.13581, "vx":0.03513, "vy":-1.7994, "omega":0.01316, "ax":0.0, "ay":0.0, "alpha":0.00521, "fx":[0.0247,0.02442,-0.0247,-0.02442], "fy":[-0.02442,0.0247,0.02442,-0.0247]},
-    {"t":1.80672, "x":2.35729, "y":4.41693, "heading":-3.13489, "vx":0.03513, "vy":-1.7994, "omega":0.01352, "ax":0.0, "ay":0.0, "alpha":0.00378, "fx":[0.01794,0.0177,-0.01794,-0.0177], "fy":[-0.0177,0.01794,0.0177,-0.01794]},
-    {"t":1.8765, "x":2.35975, "y":4.29138, "heading":-3.13395, "vx":0.03513, "vy":-1.7994, "omega":0.01378, "ax":0.0, "ay":0.0, "alpha":0.0025, "fx":[0.01187,0.01169,-0.01187,-0.01169], "fy":[-0.01169,0.01187,0.01169,-0.01187]},
-    {"t":1.94627, "x":2.3622, "y":4.16583, "heading":-3.13299, "vx":0.03513, "vy":-1.7994, "omega":0.01396, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[0.00624,0.00613,-0.00624,-0.00613], "fy":[-0.00613,0.00624,0.00613,-0.00624]},
-    {"t":2.01604, "x":2.36465, "y":4.04028, "heading":-3.13202, "vx":0.03513, "vy":-1.7994, "omega":0.01405, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[0.00084,0.00083,-0.00084,-0.00083], "fy":[-0.00083,0.00084,0.00083,-0.00084]},
-    {"t":2.08582, "x":2.3671, "y":3.91473, "heading":-3.13104, "vx":0.03513, "vy":-1.7994, "omega":0.01406, "ax":0.0, "ay":0.0, "alpha":-0.00095, "fx":[-0.00453,-0.00444,0.00453,0.00444], "fy":[0.00444,-0.00453,-0.00444,0.00453]},
-    {"t":2.15559, "x":2.36955, "y":3.78918, "heading":-3.13005, "vx":0.03513, "vy":-1.7994, "omega":0.014, "ax":0.0, "ay":0.0, "alpha":-0.00212, "fx":[-0.01009,-0.00986,0.01009,0.00986], "fy":[0.00986,-0.01009,-0.00986,0.01009]},
-    {"t":2.22537, "x":2.372, "y":3.66362, "heading":-3.12908, "vx":0.03513, "vy":-1.7994, "omega":0.01385, "ax":0.0, "ay":0.0, "alpha":-0.00336, "fx":[-0.01606,-0.01566,0.01606,0.01566], "fy":[0.01566,-0.01606,-0.01566,0.01606]},
-    {"t":2.29514, "x":2.37445, "y":3.53807, "heading":-3.12811, "vx":0.03513, "vy":-1.7994, "omega":0.01361, "ax":0.0, "ay":0.0, "alpha":-0.00474, "fx":[-0.02265,-0.02205,0.02265,0.02205], "fy":[0.02205,-0.02265,-0.02205,0.02265]},
-    {"t":2.36491, "x":2.3769, "y":3.41252, "heading":-3.12716, "vx":0.03513, "vy":-1.7994, "omega":0.01328, "ax":0.0, "ay":0.0, "alpha":-0.0063, "fx":[-0.03013,-0.02927,0.03013,0.02927], "fy":[0.02927,-0.03013,-0.02927,0.03013]},
-    {"t":2.43469, "x":2.37936, "y":3.28697, "heading":-3.12623, "vx":0.03513, "vy":-1.7994, "omega":0.01284, "ax":0.0, "ay":0.0, "alpha":-0.0081, "fx":[-0.03879,-0.03762,0.03879,0.03762], "fy":[0.03762,-0.03879,-0.03762,0.03879]},
-    {"t":2.50446, "x":2.38181, "y":3.16142, "heading":-3.12534, "vx":0.03513, "vy":-1.7994, "omega":0.01228, "ax":0.0, "ay":0.0, "alpha":-0.01021, "fx":[-0.04896,-0.0474,0.04896,0.0474], "fy":[0.0474,-0.04896,-0.0474,0.04896]},
-    {"t":2.57424, "x":2.38426, "y":3.03587, "heading":-3.12448, "vx":0.03513, "vy":-1.7994, "omega":0.01157, "ax":0.0, "ay":0.0, "alpha":-0.01272, "fx":[-0.06104,-0.05899,0.06104,0.05899], "fy":[0.05899,-0.06104,-0.05899,0.06104]},
-    {"t":2.64401, "x":2.38671, "y":2.91032, "heading":-3.12368, "vx":0.03513, "vy":-1.7994, "omega":0.01068, "ax":0.0, "ay":0.0, "alpha":-0.01572, "fx":[-0.0755,-0.07284,0.0755,0.07284], "fy":[0.07284,-0.0755,-0.07284,0.0755]},
-    {"t":2.71378, "x":2.38916, "y":2.78477, "heading":-3.12293, "vx":0.03513, "vy":-1.7994, "omega":0.00958, "ax":0.0, "ay":0.0, "alpha":-0.01933, "fx":[-0.09289,-0.08949,0.09289,0.08949], "fy":[0.08949,-0.09289,-0.08949,0.09289]},
-    {"t":2.78356, "x":2.39161, "y":2.65922, "heading":-3.12226, "vx":0.03513, "vy":-1.7994, "omega":0.00823, "ax":0.0, "ay":0.0, "alpha":-0.02369, "fx":[-0.11389,-0.10957,0.11389,0.10957], "fy":[0.10957,-0.11389,-0.10957,0.11389]},
-    {"t":2.85333, "x":2.39406, "y":2.53366, "heading":-3.12169, "vx":0.03513, "vy":-1.7994, "omega":0.00658, "ax":0.0, "ay":0.0, "alpha":-0.02896, "fx":[-0.13931,-0.13387,0.13931,0.13387], "fy":[0.13387,-0.13931,-0.13387,0.13931]},
-    {"t":2.92311, "x":2.39651, "y":2.40811, "heading":-3.12123, "vx":0.03513, "vy":-1.7994, "omega":0.00456, "ax":0.0, "ay":0.00001, "alpha":-0.03535, "fx":[-0.17012,-0.16333,0.17012,0.16333], "fy":[0.16345,-0.16999,-0.1632,0.17025]},
-    {"t":2.99288, "x":2.39896, "y":2.28256, "heading":-3.12091, "vx":0.03513, "vy":-1.7994, "omega":0.00209, "ax":-0.06289, "ay":3.22119, "alpha":-0.02977, "fx":[-1.20816,-1.20362,-0.87107,-0.87626], "fy":[53.36877,53.13929,53.14605,53.37552]},
-    {"t":3.06265, "x":2.40126, "y":2.16485, "heading":-3.12076, "vx":0.03074, "vy":-1.57464, "omega":0.00001, "ax":-0.14676, "ay":7.51686, "alpha":-0.0001, "fx":[-2.42735,-2.42735,-2.42543,-2.42543], "fy":[124.2796,124.2796,124.27964,124.27964]},
-    {"t":3.13243, "x":2.40305, "y":2.07328, "heading":-3.12076, "vx":0.0205, "vy":-1.05016, "omega":0.00001, "ax":-0.1469, "ay":7.52423, "alpha":-0.00006, "fx":[-2.42933,-2.42933,-2.4282,-2.4282], "fy":[124.40143,124.40143,124.40145,124.40145]},
-    {"t":3.2022, "x":2.40412, "y":2.01832, "heading":-3.12076, "vx":0.01025, "vy":-0.52517, "omega":0.0, "ax":-0.14695, "ay":7.52669, "alpha":-0.00005, "fx":[-2.42999,-2.42999,-2.42913,-2.42913], "fy":[124.4421,124.4421,124.44212,124.44212]},
-    {"t":3.27198, "x":2.40448, "y":2.0, "heading":-3.12076, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
-  "splits":[0]
- },
- "events":[]
-}
diff --git a/src/main/deploy/choreo/DtoS.traj b/src/main/deploy/choreo/DtoS.traj
new file mode 100644
index 00000000..77fb9b08
--- /dev/null
+++ b/src/main/deploy/choreo/DtoS.traj
@@ -0,0 +1,112 @@
+{
+ "name":"DtoS",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":0.703999767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.1980347633361816, "y":5.884655952453613, "heading":3.141666845264474, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
+    {"from":2, "to":0, "data":{"type":"MaxVelocity", "props":{"max":1.8}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"D.x", "val":0.703999767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.1980347633361816 m", "val":2.1980347633361816}, "y":{"exp":"5.884655952453613 m", "val":5.884655952453613}, "heading":{"exp":"3.1416668452644734 rad", "val":3.141666845264474}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":2, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.8 m / s", "val":1.8}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2761996,
+    "y":0.2761996
+   },
+   "backLeft":{
+    "x":-0.2761996,
+    "y":0.2761996
+   },
+   "mass":66.133767546,
+   "inertia":10.42384825260472,
+   "gearing":7.03,
+   "radius":0.0508,
+   "vmax":485.8996637552213,
+   "tmax":0.9,
+   "cof":2.25,
+   "bumper":{
+    "front":0.44196,
+    "side":0.44196,
+    "back":0.44196
+   },
+   "differentialTrackWidth":0.5427218
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.96945,3.2719],
+  "samples":[
+    {"t":0.0, "x":0.704, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.52414, "ay":0.2021, "alpha":0.0, "fx":[124.39987,124.39987,124.39987,124.39987], "fy":[3.34146,3.34148,3.34148,3.34146]},
+    {"t":0.05386, "x":0.71491, "y":5.97554, "heading":3.14159, "vx":0.40524, "vy":0.01088, "omega":0.0, "ax":7.52207, "ay":0.20205, "alpha":0.0, "fx":[124.36574,124.36574,124.36574,124.36574], "fy":[3.34054,3.34056,3.34056,3.34054]},
+    {"t":0.10772, "x":0.74765, "y":5.97642, "heading":-3.14159, "vx":0.81036, "vy":0.02177, "omega":0.0, "ax":7.51795, "ay":0.20194, "alpha":0.0, "fx":[124.29755,124.29755,124.29755,124.29755], "fy":[3.3387,3.33874,3.33874,3.33871]},
+    {"t":0.16157, "x":0.8022, "y":5.97788, "heading":-3.14159, "vx":1.21526, "vy":0.03264, "omega":0.0, "ax":7.50561, "ay":0.20161, "alpha":0.0, "fx":[124.09355,124.09355,124.09355,124.09355], "fy":[3.33321,3.33327,3.33327,3.33322]},
+    {"t":0.21543, "x":0.87853, "y":5.97994, "heading":-3.14159, "vx":1.6195, "vy":0.0435, "omega":0.0, "ax":3.33308, "ay":0.08953, "alpha":0.00049, "fx":[55.10908,55.10903,55.10534,55.10539], "fy":[1.47746,1.48293,1.48298,1.4775]},
+    {"t":0.26929, "x":0.97059, "y":5.98241, "heading":-3.14159, "vx":1.79902, "vy":0.04832, "omega":0.00003, "ax":0.00002, "ay":0.0, "alpha":0.00059, "fx":[0.00317,0.00317,-0.00242,-0.00242], "fy":[-0.00278,0.0028,0.0028,-0.00278]},
+    {"t":0.32315, "x":1.06748, "y":5.98501, "heading":-3.14159, "vx":1.79902, "vy":0.04832, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00048, "fx":[0.00224,0.00224,-0.00224,-0.00224], "fy":[-0.00224,0.00224,0.00224,-0.00224]},
+    {"t":0.37701, "x":1.16437, "y":5.98761, "heading":-3.14159, "vx":1.79902, "vy":0.04832, "omega":0.00008, "ax":0.0, "ay":0.0, "alpha":0.00037, "fx":[0.00174,0.00174,-0.00174,-0.00174], "fy":[-0.00174,0.00174,0.00174,-0.00174]},
+    {"t":0.43087, "x":1.26127, "y":5.99022, "heading":-3.14158, "vx":1.79902, "vy":0.04832, "omega":0.0001, "ax":0.0, "ay":0.0, "alpha":0.00027, "fx":[0.00128,0.00128,-0.00128,-0.00128], "fy":[-0.00128,0.00128,0.00128,-0.00128]},
+    {"t":0.48472, "x":1.35816, "y":5.99282, "heading":-3.14158, "vx":1.79902, "vy":0.04832, "omega":0.00012, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[0.00085,0.00085,-0.00085,-0.00085], "fy":[-0.00086,0.00085,0.00085,-0.00086]},
+    {"t":0.53858, "x":1.45505, "y":5.99542, "heading":-3.14157, "vx":1.79902, "vy":0.04832, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00045,0.00045,-0.00044,-0.00044], "fy":[-0.00049,0.00039,0.00039,-0.00049]},
+    {"t":0.59244, "x":1.55194, "y":5.99802, "heading":-3.14156, "vx":1.79902, "vy":0.04832, "omega":0.00013, "ax":0.0, "ay":-0.00011, "alpha":0.00001, "fx":[0.00009,0.00009,0.0,0.0], "fy":[-0.00187,-0.00178,-0.00178,-0.00187]},
+    {"t":0.6463, "x":1.64883, "y":6.00063, "heading":-3.14156, "vx":1.79902, "vy":0.04832, "omega":0.00013, "ax":0.00011, "ay":-0.004, "alpha":-0.00007, "fx":[0.00142,0.00142,0.00213,0.00213], "fy":[-0.06576,-0.06647,-0.06647,-0.06576]},
+    {"t":0.70016, "x":1.74572, "y":6.00322, "heading":-3.14155, "vx":1.79902, "vy":0.0481, "omega":0.00013, "ax":0.00355, "ay":-0.1447, "alpha":-0.00016, "fx":[0.05799,0.05799,0.05951,0.05951], "fy":[-2.3917,-2.39322,-2.39322,-2.3917]},
+    {"t":0.75401, "x":1.84262, "y":6.0056, "heading":-3.14154, "vx":1.79921, "vy":0.04031, "omega":0.00012, "ax":-0.13728, "ay":-3.86361, "alpha":-0.00015, "fx":[-2.2706,-2.27057,-2.26883,-2.26885], "fy":[-63.87821,-63.87923,-63.87925,-63.87823]},
+    {"t":0.80787, "x":1.93933, "y":6.00217, "heading":-3.14154, "vx":1.79182, "vy":-0.16778, "omega":0.00011, "ax":-1.48279, "ay":-7.18622, "alpha":-0.00001, "fx":[-24.51567,-24.51564,-24.51548,-24.51551], "fy":[-118.81287,-118.81288,-118.81291,-118.8129]},
+    {"t":0.86173, "x":2.03368, "y":5.98271, "heading":-3.14153, "vx":1.71196, "vy":-0.55482, "omega":0.00011, "ax":-3.08961, "ay":-6.81149, "alpha":0.0, "fx":[-51.08197,-51.08195,-51.0819,-51.08192], "fy":[-112.6173,-112.61731,-112.61733,-112.61732]},
+    {"t":0.91559, "x":2.1214, "y":5.94295, "heading":-3.14152, "vx":1.54556, "vy":-0.92167, "omega":0.00011, "ax":-4.55565, "ay":-5.96827, "alpha":0.0, "fx":[-75.32057,-75.32056,-75.32054,-75.32055], "fy":[-98.67609,-98.6761,-98.67611,-98.6761]},
+    {"t":0.96945, "x":2.19803, "y":5.88466, "heading":-3.14152, "vx":1.3002, "vy":-1.24311, "omega":0.00011, "ax":-5.9411, "ay":-4.59733, "alpha":0.0, "fx":[-98.22677,-98.22675,-98.22674,-98.22676], "fy":[-76.00968,-76.0097,-76.00971,-76.00969]},
+    {"t":1.0414, "x":2.27621, "y":5.78331, "heading":-3.14151, "vx":0.87273, "vy":-1.5739, "omega":0.00011, "ax":-6.99114, "ay":-2.60535, "alpha":-0.00001, "fx":[-115.58758,-115.58755,-115.58754,-115.58757], "fy":[-43.07542,-43.07551,-43.07554,-43.07545]},
+    {"t":1.11335, "x":2.32091, "y":5.66332, "heading":-3.1415, "vx":0.36971, "vy":-1.76135, "omega":0.00011, "ax":-4.56478, "ay":-0.52704, "alpha":-0.0002, "fx":[-75.4722,-75.47204,-75.47083,-75.47099], "fy":[-8.71235,-8.71495,-8.71511,-8.71252]},
+    {"t":1.1853, "x":2.33569, "y":5.53523, "heading":-3.14149, "vx":0.04126, "vy":-1.79928, "omega":0.0001, "ax":-0.08743, "ay":-0.00185, "alpha":-0.00036, "fx":[-1.4472,-1.4472,-1.4438,-1.4438], "fy":[-0.02894,-0.03234,-0.03234,-0.02894]},
+    {"t":1.25725, "x":2.33843, "y":5.40576, "heading":-3.14149, "vx":0.03497, "vy":-1.79941, "omega":0.00007, "ax":-0.00105, "ay":-0.00002, "alpha":-0.00029, "fx":[-0.01874,-0.01874,-0.01596,-0.01596], "fy":[0.00105,-0.00172,-0.00172,0.00105]},
+    {"t":1.32921, "x":2.34095, "y":5.27629, "heading":-3.14148, "vx":0.0349, "vy":-1.79941, "omega":0.00005, "ax":-0.00001, "ay":0.0, "alpha":-0.00024, "fx":[-0.00134,-0.00134,0.00092,0.00092], "fy":[0.00113,-0.00114,-0.00114,0.00113]},
+    {"t":1.40116, "x":2.34346, "y":5.14682, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[-0.00092,-0.00092,0.00092,0.00092], "fy":[0.00092,-0.00092,-0.00092,0.00092]},
+    {"t":1.47311, "x":2.34597, "y":5.01735, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[-0.00075,-0.00075,0.00075,0.00075], "fy":[0.00075,-0.00075,-0.00075,0.00075]},
+    {"t":1.54506, "x":2.34848, "y":4.88788, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00013, "fx":[-0.00061,-0.00061,0.00061,0.00061], "fy":[0.00061,-0.00061,-0.00061,0.00061]},
+    {"t":1.61701, "x":2.35099, "y":4.75841, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00049,-0.00049,0.00049,0.00049], "fy":[0.00049,-0.00049,-0.00049,0.00049]},
+    {"t":1.68896, "x":2.3535, "y":4.62894, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.0004,-0.0004,0.0004,0.0004], "fy":[0.0004,-0.0004,-0.0004,0.0004]},
+    {"t":1.76092, "x":2.35601, "y":4.49947, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00032,-0.00032,0.00032,0.00032], "fy":[0.00032,-0.00032,-0.00032,0.00032]},
+    {"t":1.83287, "x":2.35852, "y":4.37, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.00025,-0.00025,-0.00025,0.00025]},
+    {"t":1.90482, "x":2.36103, "y":4.24053, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[-0.0002,-0.0002,0.0002,0.0002], "fy":[0.0002,-0.0002,-0.0002,0.0002]},
+    {"t":1.97677, "x":2.36354, "y":4.11106, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00015,-0.00015,0.00015,0.00015], "fy":[0.00015,-0.00015,-0.00015,0.00015]},
+    {"t":2.04872, "x":2.36605, "y":3.98159, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00011,-0.00011,0.00011,0.00011], "fy":[0.00011,-0.00011,-0.00011,0.00011]},
+    {"t":2.12067, "x":2.36856, "y":3.85211, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00007,-0.00007,0.00007,0.00007], "fy":[0.00007,-0.00007,-0.00007,0.00007]},
+    {"t":2.19262, "x":2.37108, "y":3.72264, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,-0.00004,0.00004,0.00004], "fy":[0.00004,-0.00004,-0.00004,0.00004]},
+    {"t":2.26458, "x":2.37359, "y":3.59317, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,0.00001,0.00001], "fy":[0.00001,-0.00001,-0.00001,0.00001]},
+    {"t":2.33653, "x":2.3761, "y":3.4637, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,-0.00002,-0.00002], "fy":[-0.00002,0.00002,0.00002,-0.00002]},
+    {"t":2.40848, "x":2.37861, "y":3.33423, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00005,0.00005,-0.00005,-0.00005], "fy":[-0.00005,0.00005,0.00005,-0.00005]},
+    {"t":2.48043, "x":2.38112, "y":3.20476, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[0.00009,0.00009,-0.00009,-0.00009], "fy":[-0.00009,0.00009,0.00009,-0.00009]},
+    {"t":2.55238, "x":2.38363, "y":3.07529, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00013,0.00013,-0.00013,-0.00013], "fy":[-0.00013,0.00013,0.00013,-0.00013]},
+    {"t":2.62433, "x":2.38614, "y":2.94582, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.00017,0.00017,-0.00017,-0.00017], "fy":[-0.00017,0.00017,0.00017,-0.00017]},
+    {"t":2.69629, "x":2.38865, "y":2.81635, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[0.00022,0.00022,-0.00022,-0.00022], "fy":[-0.00022,0.00022,0.00022,-0.00022]},
+    {"t":2.76824, "x":2.39116, "y":2.68688, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00028,0.00028,-0.00028,-0.00028], "fy":[-0.00028,0.00028,0.00028,-0.00028]},
+    {"t":2.84019, "x":2.39367, "y":2.55741, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[0.00035,0.00035,-0.00035,-0.00035], "fy":[-0.00035,0.00035,0.00035,-0.00035]},
+    {"t":2.91214, "x":2.39618, "y":2.42794, "heading":-3.14151, "vx":0.03489, "vy":-1.79941, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00043,0.00043,-0.00043,-0.00043], "fy":[-0.00035,0.00051,0.00051,-0.00035]},
+    {"t":2.98409, "x":2.39869, "y":2.29847, "heading":-3.14151, "vx":0.03489, "vy":-1.79941, "omega":-0.00001, "ax":-0.0473, "ay":2.43926, "alpha":0.00009, "fx":[-0.78161,-0.7816,-0.78256,-0.78256], "fy":[40.32899,40.32976,40.32976,40.32899]},
+    {"t":3.05604, "x":2.40108, "y":2.17531, "heading":-3.14151, "vx":0.03149, "vy":-1.6239, "omega":0.0, "ax":-0.14579, "ay":7.5177, "alpha":0.0, "fx":[-2.41035,-2.41035,-2.41035,-2.41035], "fy":[124.29345,124.29345,124.29345,124.29345]},
+    {"t":3.128, "x":2.40297, "y":2.07793, "heading":-3.14151, "vx":0.021, "vy":-1.08299, "omega":0.0, "ax":-0.14592, "ay":7.52466, "alpha":0.0, "fx":[-2.41258,-2.41258,-2.41258,-2.41258], "fy":[124.40858,124.40858,124.40858,124.40858]},
+    {"t":3.19995, "x":2.4041, "y":2.01948, "heading":-3.14151, "vx":0.0105, "vy":-0.54158, "omega":0.0, "ax":-0.14597, "ay":7.52699, "alpha":0.0, "fx":[-2.41332,-2.41332,-2.41333,-2.41333], "fy":[124.44699,124.44699,124.44699,124.44699]},
+    {"t":3.2719, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj
new file mode 100644
index 00000000..c032e9db
--- /dev/null
+++ b/src/main/deploy/choreo/NewPath.traj
@@ -0,0 +1,28 @@
+{
+ "name":"NewPath",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/OtoS.traj b/src/main/deploy/choreo/OtoS.traj
new file mode 100644
index 00000000..ec1d1376
--- /dev/null
+++ b/src/main/deploy/choreo/OtoS.traj
@@ -0,0 +1,104 @@
+{
+ "name":"OtoS",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2761996,
+    "y":0.2761996
+   },
+   "backLeft":{
+    "x":-0.2761996,
+    "y":0.2761996
+   },
+   "mass":66.133767546,
+   "inertia":10.42384825260472,
+   "gearing":7.03,
+   "radius":0.0508,
+   "vmax":485.8996637552213,
+   "tmax":0.9,
+   "cof":2.25,
+   "bumper":{
+    "front":0.44196,
+    "side":0.44196,
+    "back":0.44196
+   },
+   "differentialTrackWidth":0.5427218
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.60553],
+  "samples":[
+    {"t":0.0, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.9745, "ay":4.57347, "alpha":0.01414, "fx":[98.794,98.89248,98.7642,98.66541], "fy":[75.59535,75.46689,75.63502,75.76343]},
+    {"t":0.03649, "x":0.73616, "y":0.72291, "heading":1.5708, "vx":0.21801, "vy":0.16688, "omega":0.00052, "ax":5.97271, "ay":4.57211, "alpha":0.01647, "fx":[98.76687,98.8816,98.73226,98.61711], "fy":[75.56952,75.41995,75.6158,75.76531]},
+    {"t":0.07298, "x":0.74809, "y":0.73204, "heading":1.57082, "vx":0.43594, "vy":0.33372, "omega":0.00112, "ax":5.96973, "ay":4.56984, "alpha":0.02035, "fx":[98.72166,98.86344,98.67912,98.5367], "fy":[75.52649,75.3418,75.58382,75.7684]},
+    {"t":0.10947, "x":0.76797, "y":0.74726, "heading":1.57086, "vx":0.65378, "vy":0.50047, "omega":0.00186, "ax":5.96379, "ay":4.56531, "alpha":0.02811, "fx":[98.63134,98.82713,98.57321,98.37621], "fy":[75.44051,75.18592,75.52009,75.77448]},
+    {"t":0.14596, "x":0.7958, "y":0.76856, "heading":1.57092, "vx":0.87139, "vy":0.66705, "omega":0.00289, "ax":5.94606, "ay":4.5518, "alpha":0.05128, "fx":[98.36118,98.71817,98.25857,97.89759], "fy":[75.18342,74.72165,75.33066,75.79181]},
+    {"t":0.18245, "x":0.83155, "y":0.79593, "heading":1.57103, "vx":1.08836, "vy":0.83314, "omega":0.00476, "ax":2.80651, "ay":2.14746, "alpha":4.48169, "fx":[30.13883,70.95579,61.57748,22.93283], "fy":[15.20348,10.96703,52.25575,63.59309]},
+    {"t":0.21894, "x":0.87313, "y":0.82776, "heading":1.5712, "vx":1.19077, "vy":0.9115, "omega":0.16829, "ax":-0.00021, "ay":0.00043, "alpha":6.26071, "fx":[-29.5273,29.54449,29.51888,-29.54975], "fy":[-29.53966,-29.51642,29.55457,29.52976]},
+    {"t":0.25542, "x":0.91658, "y":0.86102, "heading":1.57734, "vx":1.19076, "vy":0.91152, "omega":0.39674, "ax":-0.00002, "ay":0.00003, "alpha":5.74522, "fx":[-26.92562,27.27987,26.92487,-27.28045], "fy":[-27.2797,-26.92485,27.28062,26.92565]},
+    {"t":0.29191, "x":0.96003, "y":0.89428, "heading":1.59182, "vx":1.19076, "vy":0.91152, "omega":0.60638, "ax":0.0, "ay":0.0, "alpha":5.24054, "fx":[-24.19727,25.23668,24.19722,-25.23672], "fy":[-25.23667,-24.19722,25.23673,24.19727]},
+    {"t":0.3284, "x":1.00348, "y":0.92754, "heading":1.61395, "vx":1.19076, "vy":0.91152, "omega":0.7976, "ax":0.0, "ay":0.0, "alpha":4.74949, "fx":[-21.41849,23.35154,21.4185,-23.35154], "fy":[-23.35155,-21.4185,23.35154,21.41849]},
+    {"t":0.36489, "x":1.04693, "y":0.9608, "heading":1.64305, "vx":1.19076, "vy":0.91152, "omega":0.97091, "ax":0.0, "ay":0.0, "alpha":4.27441, "fx":[-18.65632,21.56776,18.65633,-21.56775], "fy":[-21.56777,-18.65634,21.56774,18.65631]},
+    {"t":0.40138, "x":1.09038, "y":0.99406, "heading":1.67848, "vx":1.19076, "vy":0.91152, "omega":1.12688, "ax":0.0, "ay":0.0, "alpha":3.81707, "fx":[-15.96755,19.83817,15.96757,-19.83815], "fy":[-19.83818,-15.96758,19.83815,15.96755]},
+    {"t":0.43787, "x":1.13383, "y":1.02733, "heading":1.7196, "vx":1.19076, "vy":0.91152, "omega":1.26616, "ax":0.0, "ay":0.0, "alpha":3.37858, "fx":[-13.3995,18.12539,13.39953,-18.12536], "fy":[-18.12539,-13.39953,18.12535,13.39949]},
+    {"t":0.47436, "x":1.17728, "y":1.06059, "heading":1.7658, "vx":1.19076, "vy":0.91152, "omega":1.38944, "ax":0.0, "ay":0.0, "alpha":2.95935, "fx":[-10.99106,16.40143,10.99109,-16.4014], "fy":[-16.40144,-10.9911,16.40139,10.99105]},
+    {"t":0.51085, "x":1.22073, "y":1.09385, "heading":1.8165, "vx":1.19076, "vy":0.91152, "omega":1.49743, "ax":0.0, "ay":0.0, "alpha":2.55912, "fx":[-8.77358,14.64668,8.77362,-14.64665], "fy":[-14.64669,-8.77363,14.64664,8.77358]},
+    {"t":0.54734, "x":1.26418, "y":1.12711, "heading":1.87114, "vx":1.19076, "vy":0.91152, "omega":1.59081, "ax":0.0, "ay":0.0, "alpha":2.17697, "fx":[-6.77186,12.84852,6.77188,-12.8485], "fy":[-12.84853,-6.77189,12.8485,6.77186]},
+    {"t":0.58383, "x":1.30763, "y":1.16037, "heading":1.92918, "vx":1.19076, "vy":0.91152, "omega":1.67024, "ax":0.0, "ay":0.0, "alpha":1.81147, "fx":[-5.00518,11.00025,5.00518,-11.00025], "fy":[-11.00025,-5.00519,11.00025,5.00518]},
+    {"t":0.62032, "x":1.35108, "y":1.19363, "heading":1.99013, "vx":1.19076, "vy":0.91152, "omega":1.73634, "ax":0.0, "ay":0.0, "alpha":1.46072, "fx":[-3.48827,9.09961,3.48825,-9.09963], "fy":[-9.09961,-3.48825,9.09963,3.48827]},
+    {"t":0.65681, "x":1.39453, "y":1.22689, "heading":2.05349, "vx":1.19076, "vy":0.91152, "omega":1.78964, "ax":0.0, "ay":0.0, "alpha":1.12245, "fx":[-2.23239,7.14805,2.23234,-7.14809], "fy":[-7.14804,-2.23233,7.1481,2.2324]},
+    {"t":0.6933, "x":1.43798, "y":1.26015, "heading":2.11879, "vx":1.19076, "vy":0.91152, "omega":1.8306, "ax":0.0, "ay":0.0, "alpha":0.79415, "fx":[-1.24608,5.14964,1.24601,-5.14971], "fy":[-5.14963,-1.246,5.14972,1.24609]},
+    {"t":0.72979, "x":1.48143, "y":1.29341, "heading":2.18559, "vx":1.19076, "vy":0.91152, "omega":1.85958, "ax":0.0, "ay":0.0, "alpha":0.47313, "fx":[-0.53596,3.11066,0.53587,-3.11075], "fy":[-3.11065,-0.53586,3.11076,0.53597]},
+    {"t":0.76627, "x":1.52488, "y":1.32667, "heading":2.25344, "vx":1.19076, "vy":0.91152, "omega":1.87684, "ax":0.0, "ay":0.0, "alpha":0.15656, "fx":[-0.10718,1.03892,0.10709,-1.03901], "fy":[-1.03891,-0.10707,1.03902,0.10719]},
+    {"t":0.80276, "x":1.56833, "y":1.35993, "heading":2.32193, "vx":1.19076, "vy":0.91152, "omega":1.88255, "ax":0.0, "ay":0.0, "alpha":-0.15846, "fx":[0.03618,-1.0566,-0.03626,1.05652], "fy":[1.05661,0.03627,-1.05651,-0.03617]},
+    {"t":0.83925, "x":1.61178, "y":1.39319, "heading":2.39062, "vx":1.19076, "vy":0.91152, "omega":1.87677, "ax":0.0, "ay":0.0, "alpha":-0.47484, "fx":[-0.10907,-3.16611,0.10901,3.16605], "fy":[3.16611,-0.109,-3.16605,0.10907]},
+    {"t":0.87574, "x":1.65523, "y":1.42645, "heading":2.4591, "vx":1.19076, "vy":0.91152, "omega":1.85944, "ax":0.0, "ay":0.0, "alpha":-0.79551, "fx":[-0.54522,-5.27928,0.5452,5.27926], "fy":[5.27928,-0.54519,-5.27926,0.54522]},
+    {"t":0.91223, "x":1.69868, "y":1.45971, "heading":2.52695, "vx":1.19076, "vy":0.91152, "omega":1.83042, "ax":0.0, "ay":0.0, "alpha":-1.12335, "fx":[-1.27354,-7.38555,1.27355,7.38557], "fy":[7.38555,-1.27355,-7.38557,1.27354]},
+    {"t":0.94872, "x":1.74213, "y":1.49297, "heading":2.59374, "vx":1.19076, "vy":0.91152, "omega":1.78943, "ax":0.0, "ay":0.0, "alpha":-1.46112, "fx":[-2.29389,-9.47426,2.29393,9.4743], "fy":[9.47426,-2.29393,-9.4743,2.29389]},
+    {"t":0.98521, "x":1.78558, "y":1.52623, "heading":2.65904, "vx":1.19076, "vy":0.91152, "omega":1.73611, "ax":0.0, "ay":0.0, "alpha":-1.81142, "fx":[-3.60416,-11.53506,3.60421,11.53511], "fy":[11.53505,-3.60422,-11.53512,3.60415]},
+    {"t":1.0217, "x":1.82903, "y":1.5595, "heading":2.72239, "vx":1.19076, "vy":0.91152, "omega":1.67001, "ax":0.0, "ay":0.0, "alpha":-2.17653, "fx":[-5.19939,-13.55816,5.19945,13.55822], "fy":[13.55815,-5.19946,-13.55822,5.19939]},
+    {"t":1.05819, "x":1.87248, "y":1.59276, "heading":2.78332, "vx":1.19076, "vy":0.91152, "omega":1.59059, "ax":0.0, "ay":0.0, "alpha":-2.55842, "fx":[-7.07089,-15.53524,7.07094,15.53529], "fy":[15.53523,-7.07094,-15.5353,7.07088]},
+    {"t":1.09468, "x":1.91593, "y":1.62602, "heading":2.84136, "vx":1.19076, "vy":0.91152, "omega":1.49724, "ax":0.0, "ay":0.0, "alpha":-2.95852, "fx":[-9.20499,-17.46017,9.20503,17.46021], "fy":[17.46017,-9.20503,-17.46021,9.20498]},
+    {"t":1.13117, "x":1.95938, "y":1.65928, "heading":2.896, "vx":1.19076, "vy":0.91152, "omega":1.38928, "ax":0.0, "ay":0.0, "alpha":-3.37772, "fx":[-11.58213,-19.33056,11.58216,19.33059], "fy":[19.33056,-11.58216,-19.33059,11.58213]},
+    {"t":1.16766, "x":2.00283, "y":1.69254, "heading":2.94669, "vx":1.19076, "vy":0.91152, "omega":1.26603, "ax":0.0, "ay":0.0, "alpha":-3.81627, "fx":[-14.17581,-21.14923,14.17582,21.14924], "fy":[21.14923,-14.17583,-21.14924,14.17581]},
+    {"t":1.20415, "x":2.04628, "y":1.7258, "heading":2.99289, "vx":1.19076, "vy":0.91152, "omega":1.12678, "ax":0.0, "ay":0.0, "alpha":-4.27372, "fx":[-16.95187,-22.92599,16.95188,22.926], "fy":[22.92599,-16.95188,-22.926,16.95187]},
+    {"t":1.24064, "x":2.08973, "y":1.75906, "heading":3.034, "vx":1.19076, "vy":0.91152, "omega":0.97084, "ax":0.0, "ay":0.0, "alpha":-4.74895, "fx":[-19.86811,-24.67952,19.86811,24.67953], "fy":[24.67953,-19.86811,-24.67953,19.86811]},
+    {"t":1.27712, "x":2.13318, "y":1.79232, "heading":3.06943, "vx":1.19076, "vy":0.91152, "omega":0.79755, "ax":0.0, "ay":0.0, "alpha":-5.24016, "fx":[-22.8739,-26.43868,22.87394,26.43873], "fy":[26.43868,-22.87395,-26.43873,22.87389]},
+    {"t":1.31361, "x":2.17663, "y":1.82558, "heading":3.09853, "vx":1.19076, "vy":0.91152, "omega":0.60634, "ax":0.00002, "ay":-0.00002, "alpha":-5.74499, "fx":[-25.91009,-28.24344,25.91065,28.24415], "fy":[28.24342,-25.91082,-28.24418,25.90992]},
+    {"t":1.3501, "x":2.22008, "y":1.85884, "heading":3.12066, "vx":1.19076, "vy":0.91152, "omega":0.39671, "ax":0.00019, "ay":-0.00041, "alpha":-6.26062, "fx":[-28.90742,-30.14259,28.91235,30.15048], "fy":[30.14015,-28.91707,-30.15291,28.9027]},
+    {"t":1.38659, "x":2.26353, "y":1.8921, "heading":3.13513, "vx":1.19076, "vy":0.9115, "omega":0.16826, "ax":-2.80649, "ay":-2.14748, "alpha":-4.48101, "fx":[-70.88449,-61.71494,-23.07689,-29.92726], "fy":[-10.81428,-52.1031,-63.66827,-15.43546]},
+    {"t":1.42308, "x":2.30511, "y":1.92393, "heading":3.14127, "vx":1.08836, "vy":0.83314, "omega":0.00476, "ax":-5.94607, "ay":-4.55179, "alpha":-0.05128, "fx":[-98.71822,-98.2588,-97.89763,-98.36104], "fy":[-74.72158,-75.33036,-75.79177,-75.1836]},
+    {"t":1.45957, "x":2.34087, "y":1.9513, "heading":3.14145, "vx":0.87139, "vy":0.66705, "omega":0.00288, "ax":-5.96379, "ay":-4.56531, "alpha":-0.02811, "fx":[-98.82714,-98.57328,-98.37624,-98.63132], "fy":[-75.1859,-75.51999,-75.77444,-75.44054]},
+    {"t":1.49606, "x":2.36869, "y":1.9726, "heading":3.14155, "vx":0.65378, "vy":0.50047, "omega":0.00186, "ax":-5.96973, "ay":-4.56984, "alpha":-0.02035, "fx":[-98.86345,-98.67915,-98.53673,-98.72167], "fy":[-75.3418,-75.58378,-75.76836,-75.52648]},
+    {"t":1.53255, "x":2.38857, "y":1.98782, "heading":-3.14157, "vx":0.43594, "vy":0.33372, "omega":0.00112, "ax":-5.97271, "ay":-4.5721, "alpha":-0.01647, "fx":[-98.88159,-98.73227,-98.61714,-98.76688], "fy":[-75.41995,-75.61578,-75.76527,-75.56951]},
+    {"t":1.56904, "x":2.4005, "y":1.99696, "heading":-3.14153, "vx":0.21801, "vy":0.16688, "omega":0.00052, "ax":-5.9745, "ay":-4.57347, "alpha":-0.01413, "fx":[-98.89248,-98.76421,-98.66544,-98.79402], "fy":[-75.4669,-75.63501,-75.76339,-75.59533]},
+    {"t":1.60553, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/PMtoM.traj b/src/main/deploy/choreo/PMtoM.traj
new file mode 100644
index 00000000..2a744b63
--- /dev/null
+++ b/src/main/deploy/choreo/PMtoM.traj
@@ -0,0 +1,80 @@
+{
+ "name":"PMtoM",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.674594640731811, "y":4.0000072192, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.8712037801742551, "y":4.0, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
+    {"from":1, "to":0, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"PM.x", "val":3.674594640731811}, "y":{"exp":"PM.y", "val":4.0000072192}, "heading":{"exp":"PM.heading", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"M.x", "val":1.8712037801742551}, "y":{"exp":"M.y", "val":4.0}, "heading":{"exp":"M.heading", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":1, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2761996,
+    "y":0.2761996
+   },
+   "backLeft":{
+    "x":-0.2761996,
+    "y":0.2761996
+   },
+   "mass":66.133767546,
+   "inertia":10.42384825260472,
+   "gearing":7.03,
+   "radius":0.0508,
+   "vmax":485.8996637552213,
+   "tmax":0.9,
+   "cof":2.25,
+   "bumper":{
+    "front":0.44196,
+    "side":0.44196,
+    "back":0.44196
+   },
+   "differentialTrackWidth":0.5427218
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.95293],
+  "samples":[
+    {"t":0.0, "x":3.67459, "y":4.00001, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52887, "ay":-0.00003, "alpha":0.0, "fx":[-124.47811,-124.47811,-124.47811,-124.47811], "fy":[-0.0005,-0.0005,-0.0005,-0.0005]},
+    {"t":0.09765, "x":3.6387, "y":4.00001, "heading":0.0, "vx":-0.73517, "vy":0.0, "omega":0.0, "ax":-2.71112, "ay":-0.00001, "alpha":0.0, "fx":[-44.82422,-44.82422,-44.82422,-44.82422], "fy":[-0.00018,-0.00018,-0.00018,-0.00018]},
+    {"t":0.19529, "x":3.55399, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.29294, "x":3.45635, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39059, "x":3.35872, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.48823, "x":3.26108, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.58588, "x":3.16344, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.68352, "x":3.06581, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.78117, "x":2.96817, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.87882, "x":2.87054, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.97646, "x":2.7729, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.07411, "x":2.67526, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.17176, "x":2.57763, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.2694, "x":2.47999, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.36705, "x":2.38235, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.4647, "x":2.28472, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.56234, "x":2.18708, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.65999, "x":2.08944, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.75763, "x":1.99181, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":2.71112, "ay":0.00001, "alpha":0.0, "fx":[44.82422,44.82422,44.82422,44.82422], "fy":[0.00018,0.00018,0.00018,0.00018]},
+    {"t":1.85528, "x":1.9071, "y":4.0, "heading":0.0, "vx":-0.73517, "vy":0.0, "omega":0.0, "ax":7.52887, "ay":0.00003, "alpha":0.0, "fx":[124.47811,124.47811,124.47811,124.47811], "fy":[0.0005,0.0005,0.0005,0.0005]},
+    {"t":1.95293, "x":1.8712, "y":4.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/StoO.traj b/src/main/deploy/choreo/StoO.traj
new file mode 100644
index 00000000..f9ca3948
--- /dev/null
+++ b/src/main/deploy/choreo/StoO.traj
@@ -0,0 +1,115 @@
+{
+ "name":"StoO",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.6131027936935425, "y":1.1535886526107788, "heading":1.9909931005444836, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"1.6131027936935425 m", "val":1.6131027936935425}, "y":{"exp":"1.1535886526107788 m", "val":1.1535886526107788}, "heading":{"exp":"1.9909931005444836 rad", "val":1.9909931005444836}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2761996,
+    "y":0.2761996
+   },
+   "backLeft":{
+    "x":-0.2761996,
+    "y":0.2761996
+   },
+   "mass":66.133767546,
+   "inertia":10.42384825260472,
+   "gearing":7.03,
+   "radius":0.0508,
+   "vmax":485.8996637552213,
+   "tmax":0.9,
+   "cof":2.25,
+   "bumper":{
+    "front":0.44196,
+    "side":0.44196,
+    "back":0.44196
+   },
+   "differentialTrackWidth":0.5427218
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.8731,1.62816],
+  "samples":[
+    {"t":0.0, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.10376, "ay":-5.52485, "alpha":-0.03459, "fx":[-84.72077,-84.39689,-84.04436,-84.36919], "fy":[-91.03195,-91.3333,-91.65688,-91.35696]},
+    {"t":0.0291, "x":2.40232, "y":1.99766, "heading":-3.14151, "vx":-0.14854, "vy":-0.16079, "omega":-0.00101, "ax":-5.10221, "ay":-5.52317, "alpha":-0.03904, "fx":[-84.73841,-84.37317,-83.97524,-84.3417], "fy":[-90.96396,-91.30418,-91.66906,-91.33065]},
+    {"t":0.05821, "x":2.39584, "y":1.99064, "heading":-3.14154, "vx":-0.29703, "vy":-0.32153, "omega":-0.00214, "ax":-5.0999, "ay":-5.52066, "alpha":-0.04567, "fx":[-84.76466,-84.33784,-83.87217,-84.30064], "fy":[-90.86254,-91.26081,-91.68717,-91.29138]},
+    {"t":0.08731, "x":2.38503, "y":1.97895, "heading":3.14159, "vx":-0.44545, "vy":-0.4822, "omega":-0.00347, "ax":-5.09607, "ay":-5.51651, "alpha":-0.05664, "fx":[-84.80785,-84.27963,-83.70194,-84.2327], "fy":[-90.69502,-91.1893,-91.71689,-91.22642]},
+    {"t":0.11641, "x":2.36991, "y":1.96258, "heading":3.14148, "vx":-0.59376, "vy":-0.64275, "omega":-0.00512, "ax":-5.08853, "ay":-5.50835, "alpha":-0.07822, "fx":[-84.89218,-84.16562,-83.36731,-84.09867], "fy":[-90.36556,-91.04921,-91.77473,-91.0983]},
+    {"t":0.14552, "x":2.35047, "y":1.94154, "heading":3.14134, "vx":-0.74186, "vy":-0.80306, "omega":-0.0074, "ax":-5.06686, "ay":-5.48487, "alpha":-0.14021, "fx":[-85.12967,-83.84172,-82.40806,-83.71113], "fy":[-89.42012,-90.65129,-91.93612,-90.7278]},
+    {"t":0.17462, "x":2.32674, "y":1.91584, "heading":3.14112, "vx":-0.88932, "vy":-0.96269, "omega":-0.01148, "ax":-4.4031, "ay":-4.76668, "alpha":-1.97319, "fx":[-89.21431,-76.12615,-55.64594,-70.20744], "fy":[-62.43516,-81.2333,-93.87683,-77.69326]},
+    {"t":0.20372, "x":2.29899, "y":1.88581, "heading":3.14079, "vx":-1.01747, "vy":-1.10142, "omega":-0.0689, "ax":-0.00096, "ay":-0.00086, "alpha":-9.7736, "fx":[-46.08987,-46.15639,46.05032,46.13246], "fy":[46.12587,-46.07996,-46.16299,46.06024]},
+    {"t":0.23283, "x":2.26938, "y":1.85375, "heading":3.13878, "vx":-1.01749, "vy":-1.10144, "omega":-0.35335, "ax":-0.00001, "ay":0.00001, "alpha":-9.23854, "fx":[-43.46061,-43.70578,43.46024,43.7052], "fy":[43.70565,-43.46015,-43.70533,43.4607]},
+    {"t":0.26193, "x":2.23977, "y":1.8217, "heading":3.1285, "vx":-1.01749, "vy":-1.10144, "omega":-0.62222, "ax":0.0, "ay":0.0, "alpha":-8.67153, "fx":[-40.36908,-41.44046,40.36903,41.44038], "fy":[41.44045,-40.36902,-41.4404,40.36909]},
+    {"t":0.29103, "x":2.21015, "y":1.78964, "heading":3.11039, "vx":-1.01749, "vy":-1.10144, "omega":-0.87459, "ax":0.0, "ay":0.0, "alpha":-8.07258, "fx":[-36.87601,-39.25228,36.87603,39.2523], "fy":[39.25229,-36.87602,-39.25229,36.87602]},
+    {"t":0.32014, "x":2.18054, "y":1.75758, "heading":3.08494, "vx":-1.01749, "vy":-1.10144, "omega":-1.10953, "ax":0.0, "ay":0.0, "alpha":-7.44244, "fx":[-33.06544,-37.04179,33.06549,37.04185], "fy":[37.04181,-33.06548,-37.04183,33.06544]},
+    {"t":0.34924, "x":2.15093, "y":1.72553, "heading":3.05264, "vx":-1.01749, "vy":-1.10144, "omega":-1.32613, "ax":0.0, "ay":0.0, "alpha":-6.78267, "fx":[-29.02856,-34.71332,29.02865,34.71342], "fy":[34.71334,-29.02865,-34.7134,29.02856]},
+    {"t":0.37834, "x":2.12132, "y":1.69347, "heading":3.01405, "vx":-1.01749, "vy":-1.10144, "omega":-1.52353, "ax":0.0, "ay":0.0, "alpha":-6.09563, "fx":[-24.86495,-32.18043,24.86509,32.1806], "fy":[32.18046,-24.8651,-32.18057,24.86494]},
+    {"t":0.40745, "x":2.0917, "y":1.66142, "heading":2.96971, "vx":-1.01749, "vy":-1.10144, "omega":-1.70093, "ax":0.00001, "ay":-0.00001, "alpha":-5.3844, "fx":[-20.68215,-29.37125,20.68236,29.37148], "fy":[29.37128,-20.68236,-29.37145,20.68214]},
+    {"t":0.43655, "x":2.06209, "y":1.62936, "heading":2.92021, "vx":-1.01749, "vy":-1.10144, "omega":-1.85764, "ax":0.00001, "ay":-0.00001, "alpha":-4.6526, "fx":[-16.59341,-26.23252,16.59367,26.23279], "fy":[26.23255,-16.59367,-26.23276,16.59341]},
+    {"t":0.46565, "x":2.03248, "y":1.59731, "heading":2.86614, "vx":-1.01749, "vy":-1.10144, "omega":-1.99304, "ax":0.00001, "ay":-0.00001, "alpha":-3.90401, "fx":[-12.71377,-22.73203,12.71405,22.73232], "fy":[22.73206,-12.71404,-22.73229,12.71378]},
+    {"t":0.49476, "x":2.00287, "y":1.56525, "heading":2.80814, "vx":-1.01749, "vy":-1.10144, "omega":-2.10666, "ax":0.00001, "ay":-0.00001, "alpha":-3.14246, "fx":[-9.15572,-18.86018,9.15595,18.86041], "fy":[18.8602,-9.15594,-18.86039,9.15573]},
+    {"t":0.52386, "x":1.97325, "y":1.53319, "heading":2.74683, "vx":-1.01749, "vy":-1.10144, "omega":-2.19812, "ax":0.0, "ay":0.0, "alpha":-2.37127, "fx":[-6.02387,-14.62834,6.02396,14.62844], "fy":[14.62835,-6.02395,-14.62843,6.02387]},
+    {"t":0.55296, "x":1.94364, "y":1.50114, "heading":2.68286, "vx":-1.01749, "vy":-1.10144, "omega":-2.26713, "ax":0.0, "ay":0.0, "alpha":-1.59344, "fx":[-3.41129,-10.0687,3.4112,10.06861], "fy":[10.0687,-3.41121,-10.06861,3.41129]},
+    {"t":0.58207, "x":1.91403, "y":1.46908, "heading":2.61688, "vx":-1.01749, "vy":-1.10144, "omega":-2.3135, "ax":-0.00001, "ay":0.00001, "alpha":-0.8113, "fx":[-1.3952,-5.22997,1.39492,5.22969], "fy":[5.22996,-1.39493,-5.2297,1.39519]},
+    {"t":0.61117, "x":1.88442, "y":1.43703, "heading":2.54954, "vx":-1.01749, "vy":-1.10144, "omega":-2.33712, "ax":-0.00001, "ay":0.00001, "alpha":-0.02697, "fx":[-0.03479,-0.1768,0.03436,0.17637], "fy":[0.17678,-0.03438,-0.17639,0.03477]},
+    {"t":0.64027, "x":1.8548, "y":1.40497, "heading":2.48153, "vx":-1.01749, "vy":-1.10144, "omega":-2.3379, "ax":-0.00002, "ay":0.00002, "alpha":0.75775, "fx":[0.63165,5.01548,-0.63225,-5.01607], "fy":[-5.0155,0.63223,5.01605,-0.63168]},
+    {"t":0.66938, "x":1.82519, "y":1.37292, "heading":2.41349, "vx":-1.01749, "vy":-1.10144, "omega":-2.31585, "ax":-0.00005, "ay":0.00004, "alpha":1.54086, "fx":[0.58783,10.26235,-0.58945,-10.26395], "fy":[-10.26242,0.58938,10.26388,-0.58789]},
+    {"t":0.69848, "x":1.79558, "y":1.34086, "heading":2.34609, "vx":-1.01749, "vy":-1.10144, "omega":-2.271, "ax":-0.00035, "ay":0.00032, "alpha":2.32016, "fx":[-0.16229,15.4727,0.1506,-15.48403], "fy":[-15.47313,-0.15105,15.4836,0.16184]},
+    {"t":0.72758, "x":1.76597, "y":1.3088, "heading":2.27999, "vx":-1.0175, "vy":-1.10143, "omega":-2.20348, "ax":-0.00315, "ay":0.00291, "alpha":3.09297, "fx":[-1.62466,20.52467,1.51712,-20.62573], "fy":[-20.52812,-1.52161,20.62229,1.62015]},
+    {"t":0.75669, "x":1.73635, "y":1.27675, "heading":2.21587, "vx":-1.0176, "vy":-1.10135, "omega":-2.11346, "ax":-0.02849, "ay":0.02634, "alpha":3.85602, "fx":[-4.09439,25.02633,3.1035,-25.91933], "fy":[-25.04987,-3.14977,25.89624,4.04533]},
+    {"t":0.78579, "x":1.70673, "y":1.24471, "heading":2.15436, "vx":-1.01843, "vy":-1.10058, "omega":-2.00124, "ax":-0.24876, "ay":0.23173, "alpha":4.58871, "fx":[-10.63174,26.18766,1.76741,-33.775], "fy":[-26.28888,-2.12728,33.71888,10.02229]},
+    {"t":0.81489, "x":1.67698, "y":1.21278, "heading":2.09611, "vx":-1.02567, "vy":-1.09384, "omega":-1.86769, "ax":-1.88252, "ay":1.85844, "alpha":4.24043, "fx":[-43.8043,-2.39918,-22.58399,-55.71062], "fy":[1.67396,27.93544,57.80486,35.49116]},
+    {"t":0.844, "x":1.64633, "y":1.18173, "heading":2.04176, "vx":-1.08045, "vy":-1.03975, "omega":-1.74428, "ax":-4.21636, "ay":5.00477, "alpha":0.92918, "fx":[-76.50689,-64.19914,-63.4634,-74.67412], "fy":[75.85539,85.7956,88.98499,80.34806]},
+    {"t":0.8731, "x":1.6131, "y":1.15359, "heading":1.99099, "vx":-1.20316, "vy":-0.89409, "omega":-1.71724, "ax":-3.50578, "ay":5.46433, "alpha":1.01765, "fx":[-65.80325,-50.87052,-50.94855,-64.22792], "fy":[83.99578,93.28598,96.0298,88.06493]},
+    {"t":0.90593, "x":1.57172, "y":1.12718, "heading":1.93462, "vx":-1.31825, "vy":-0.71471, "omega":-1.68383, "ax":-0.88214, "ay":1.71212, "alpha":5.14988, "fx":[-33.33245,19.77884,-0.67343,-44.11192], "fy":[-3.20684,17.30054,58.33755,40.79766]},
+    {"t":0.93876, "x":1.52796, "y":1.10464, "heading":1.87934, "vx":-1.34721, "vy":-0.6585, "omega":-1.51477, "ax":-0.08015, "ay":0.16481, "alpha":5.37733, "fx":[-18.06135,30.80225,14.91478,-32.95634], "fy":[-29.18363,-13.67837,34.55081,19.2108]},
+    {"t":0.97159, "x":1.48369, "y":1.08311, "heading":1.82961, "vx":-1.34984, "vy":-0.65309, "omega":-1.33824, "ax":-0.00715, "ay":0.01479, "alpha":4.98193, "fx":[-16.84227,28.63586,16.56709,-28.83376], "fy":[-28.49148,-16.4585,28.97798,16.95021]},
+    {"t":1.00441, "x":1.43938, "y":1.06168, "heading":1.78568, "vx":-1.35008, "vy":-0.6526, "omega":-1.17469, "ax":-0.00063, "ay":0.0013, "alpha":4.59851, "fx":[-16.58073,25.81161,16.55708,-25.82946], "fy":[-25.79904,-16.54747,25.84203,16.59034]},
+    {"t":1.03724, "x":1.39505, "y":1.04026, "heading":1.74712, "vx":-1.3501, "vy":-0.65256, "omega":-1.02372, "ax":-0.00005, "ay":0.00011, "alpha":4.23277, "fx":[-16.15705,23.16043,16.15505,-23.16201], "fy":[-23.15937,-16.15422,23.16307,16.15788]},
+    {"t":1.07007, "x":1.35073, "y":1.01883, "heading":1.71351, "vx":-1.3501, "vy":-0.65256, "omega":-0.88477, "ax":0.0, "ay":0.00001, "alpha":3.88449, "fx":[-15.53263,20.74523,15.53247,-20.74536], "fy":[-20.74514,-15.5324,20.74545,15.5327]},
+    {"t":1.1029, "x":1.30641, "y":0.99741, "heading":1.68446, "vx":-1.3501, "vy":-0.65256, "omega":-0.75724, "ax":0.0, "ay":0.0, "alpha":3.55327, "fx":[-14.75325,18.55576,14.75324,-18.55578], "fy":[-18.55576,-14.75324,18.55578,14.75326]},
+    {"t":1.13573, "x":1.26209, "y":0.97599, "heading":1.6596, "vx":-1.3501, "vy":-0.65256, "omega":-0.64059, "ax":0.0, "ay":0.0, "alpha":3.23854, "fx":[-13.86271,16.57273,13.86271,-16.57273], "fy":[-16.57273,-13.86271,16.57273,13.86271]},
+    {"t":1.16856, "x":1.21776, "y":0.95457, "heading":1.63857, "vx":-1.3501, "vy":-0.65256, "omega":-0.53428, "ax":0.0, "ay":0.0, "alpha":2.93959, "fx":[-12.89659,14.77498,12.89659,-14.77498], "fy":[-14.77498,-12.89659,14.77498,12.89659]},
+    {"t":1.20139, "x":1.17344, "y":0.93314, "heading":1.62103, "vx":-1.3501, "vy":-0.65256, "omega":-0.43777, "ax":0.0, "ay":0.0, "alpha":2.6556, "fx":[-11.88297,13.14119,11.88297,-13.14119], "fy":[-13.14119,-11.88297,13.14119,11.88297]},
+    {"t":1.23422, "x":1.12912, "y":0.91172, "heading":1.60666, "vx":-1.3501, "vy":-0.65256, "omega":-0.35059, "ax":0.0, "ay":0.0, "alpha":2.38564, "fx":[-10.84355,11.65068,10.84355,-11.65068], "fy":[-11.65068,-10.84355,11.65068,10.84355]},
+    {"t":1.26705, "x":1.0848, "y":0.8903, "heading":1.59515, "vx":-1.3501, "vy":-0.65256, "omega":-0.27227, "ax":0.0, "ay":0.0, "alpha":2.12872, "fx":[-9.79475,10.2839,9.79475,-10.2839], "fy":[-10.2839,-9.79475,10.2839,9.79475]},
+    {"t":1.29987, "x":1.04048, "y":0.86887, "heading":1.58621, "vx":-1.3501, "vy":-0.65256, "omega":-0.20239, "ax":0.0, "ay":0.0, "alpha":1.88376, "fx":[-8.74864,9.02267,8.74864,-9.02267], "fy":[-9.02267,-8.74864,9.02267,8.74864]},
+    {"t":1.3327, "x":0.99615, "y":0.84745, "heading":1.57957, "vx":-1.3501, "vy":-0.65256, "omega":-0.14055, "ax":0.0, "ay":0.0, "alpha":1.64966, "fx":[-7.71376,7.85032,7.71376,-7.85032], "fy":[-7.85032,-7.71376,7.85032,7.71376]},
+    {"t":1.36553, "x":0.95183, "y":0.82603, "heading":1.57496, "vx":-1.3501, "vy":-0.65256, "omega":-0.08639, "ax":0.00002, "ay":0.00001, "alpha":1.42528, "fx":[-6.69551,6.75198,6.69603,-6.75145], "fy":[-6.75158,-6.69564,6.75184,6.6959]},
+    {"t":1.39836, "x":0.90751, "y":0.80461, "heading":1.57212, "vx":-1.3501, "vy":-0.65256, "omega":-0.0396, "ax":0.55769, "ay":0.26955, "alpha":1.19326, "fx":[3.63455,14.88035,14.77167,3.59565], "fy":[-1.21198,-1.18075,10.0501,10.16927]},
+    {"t":1.43119, "x":0.86349, "y":0.78333, "heading":1.57082, "vx":-1.33179, "vy":-0.64371, "omega":-0.00043, "ax":6.73047, "ay":3.2531, "alpha":0.00771, "fx":[111.29183,111.32001,111.26394,111.23566], "fy":[53.75443,53.69835,53.81553,53.87168]},
+    {"t":1.46402, "x":0.82339, "y":0.76395, "heading":1.57081, "vx":-1.11084, "vy":-0.53691, "omega":-0.00018, "ax":6.75662, "ay":3.26574, "alpha":0.00301, "fx":[111.7158,111.7267,111.70459,111.69367], "fy":[53.98203,53.95991,54.00589,54.02801]},
+    {"t":1.49685, "x":0.79057, "y":0.74808, "heading":1.5708, "vx":-0.88903, "vy":-0.4297, "omega":-0.00008, "ax":6.76525, "ay":3.26991, "alpha":0.00147, "fx":[111.85563,111.86092,111.85011,111.84481], "fy":[54.0571,54.04629,54.06874,54.07956]},
+    {"t":1.52968, "x":0.76503, "y":0.73574, "heading":1.5708, "vx":-0.66693, "vy":-0.32236, "omega":-0.00003, "ax":6.76955, "ay":3.27199, "alpha":0.0007, "fx":[111.92524,111.92776,111.92259,111.92006], "fy":[54.09448,54.08931,54.10004,54.10521]},
+    {"t":1.5625, "x":0.74678, "y":0.72692, "heading":1.5708, "vx":-0.4447, "vy":-0.21494, "omega":-0.00001, "ax":6.77212, "ay":3.27323, "alpha":0.00024, "fx":[111.9669,111.96777,111.96598,111.96511], "fy":[54.11686,54.11506,54.11878,54.12057]},
+    {"t":1.59533, "x":0.73583, "y":0.72163, "heading":1.5708, "vx":-0.22238, "vy":-0.10748, "omega":0.0, "ax":6.77383, "ay":3.27406, "alpha":-0.00006, "fx":[111.99463,111.9944,111.99487,111.99509], "fy":[54.13175,54.13221,54.13125,54.13079]},
+    {"t":1.62816, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor
index b060eae2..4a4bcd90 100644
--- a/src/main/deploy/choreo/rebuiltChoreo.chor
+++ b/src/main/deploy/choreo/rebuiltChoreo.chor
@@ -34,20 +34,6 @@
    }
   },
   "poses":{
-   "CC":{
-    "x":{
-     "exp":"2.4044814109802246 m",
-     "val":2.4044814109802246
-    },
-    "y":{
-     "exp":"2 m",
-     "val":2.0
-    },
-    "heading":{
-     "exp":"-3.120762420732352 rad",
-     "val":-3.120762420732352
-    }
-   },
    "CL":{
     "x":{
      "exp":"1.998773455619812 m",
@@ -244,6 +230,20 @@
      "val":1.5707963267948966
     }
    },
+   "M":{
+    "x":{
+     "exp":"1.8712037801742554 m",
+     "val":1.8712037801742551
+    },
+    "y":{
+     "exp":"4 m",
+     "val":4.0
+    },
+    "heading":{
+     "exp":"0 rad",
+     "val":0.0
+    }
+   },
    "MLL":{
     "x":{
      "exp":"3.2225897312164307 m",
@@ -314,6 +314,20 @@
      "val":3.141592653589793
     }
    },
+   "PM":{
+    "x":{
+     "exp":"3.6745946407318115 m",
+     "val":3.674594640731811
+    },
+    "y":{
+     "exp":"4.0000072192 m",
+     "val":4.0000072192
+    },
+    "heading":{
+     "exp":"0 rad",
+     "val":0.0
+    }
+   },
    "PRB":{
     "x":{
      "exp":"3.7007737159729004 m",
@@ -370,6 +384,20 @@
      "val":0.0
     }
    },
+   "SC":{
+    "x":{
+     "exp":"2.4044814109802246 m",
+     "val":2.4044814109802246
+    },
+    "y":{
+     "exp":"2 m",
+     "val":2.0
+    },
+    "heading":{
+     "exp":"-3.1415076242073234 rad",
+     "val":-3.141507624207323
+    }
+   },
    "SLB":{
     "x":{
      "exp":"2.7844135761260986 m",
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 0d976eee..31134c42 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -105,7 +105,7 @@ public enum Path {
     // OUTPOST
     PRtoO("PR", "O", Action.OUTPOST),
     MRtoO("MR", "O", Action.OUTPOST),
-    CtoO("C", "O", Action.OUTPOST),
+    StoO("S", "O", Action.OUTPOST),
     // DEPOT
     PLtoD("PL", "D", Action.INTAKE),
     // FEED
@@ -125,7 +125,9 @@ public enum Path {
     // SCORE
     DtoRL("D", "RL", Action.SCORE),
     OtoRR("O", "RR", Action.SCORE),
-    DtoC("D", "C", Action.SCORE),
+    DtoS("D", "S", Action.SCORE),
+    OtoS("O", "S", Action.SCORE),
+    PMtoM("PM", "M", Action.SCORE),
     // FLOW
     MLtoD("ML", "D", Action.FLOW),
     // CLIMB
@@ -515,10 +517,25 @@ public Command getDepotClimbAuto() {
     return routine.cmd();
   }
 
+  public Command getOutpostClimbAuto() {
+    final AutoRoutine routine = factory.newRoutine("Outpost Climb Auto");
+    lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
+    Path[] paths = {Path.PRtoO, Path.OtoS, Path.OtoCR};
+    Command autoCommand =
+        paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse());
+
+    for (Path p : paths) {
+      autoCommand = autoCommand.andThen(runPath(p, routine));
+    }
+    routine.active().whileTrue(autoCommand);
+
+    return routine.cmd();
+  }
+
   public Command getDepotOutpostClimbAuto() {
     final AutoRoutine routine = factory.newRoutine("Depot Outpost Climb Auto");
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
-    Path[] paths = {Path.PLtoD, Path.DtoC, Path.CtoO, Path.OtoCR};
+    Path[] paths = {Path.PLtoD, Path.DtoS, Path.StoO, Path.OtoCR};
     Command autoCommand =
         paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse());
 
@@ -531,6 +548,22 @@ public Command getDepotOutpostClimbAuto() {
     return routine.cmd();
   }
 
+  public Command getCenterScoreAuto() {
+    final AutoRoutine routine = factory.newRoutine("Center Score Auto");
+    lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
+    Path[] paths = {Path.PMtoM};
+    Command autoCommand =
+        paths[0].getTrajectory(routine).resetOdometry();
+
+    for (Path p : paths) {
+      autoCommand = autoCommand.andThen(runPath(p, routine));
+    }
+
+    routine.active().whileTrue(autoCommand);
+
+    return routine.cmd();
+  }
+
   public Command getTestAuto() {
     final AutoRoutine routine = factory.newRoutine("test auto");
     Path[] paths = {Path.RUNtoTEST, Path.RUNtoTEST, Path.RUNtoTEST, Path.RUNtoTEST};

From daa94333c8c58edfd7b4df83af54d5ecbeb43228 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 13 Mar 2026 23:35:00 -0700
Subject: [PATCH 11/54] add to auto chooser

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

diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 31134c42..28479e7d 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -552,8 +552,7 @@ public Command getCenterScoreAuto() {
     final AutoRoutine routine = factory.newRoutine("Center Score Auto");
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
     Path[] paths = {Path.PMtoM};
-    Command autoCommand =
-        paths[0].getTrajectory(routine).resetOdometry();
+    Command autoCommand = paths[0].getTrajectory(routine).resetOdometry();
 
     for (Path p : paths) {
       autoCommand = autoCommand.andThen(runPath(p, routine));
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f5c3cfde..f940dcd6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -725,6 +725,8 @@ private void addAutos() {
     autoChooser.addOption("Fill Outpost Score Climb", autos.getFillOutpostScoreClimbAuto());
     autoChooser.addOption("Depot Climb", autos.getDepotClimbAuto());
     autoChooser.addOption("Depot Outpost Climb", autos.getDepotOutpostClimbAuto());
+    autoChooser.addOption("Outpost Climb", autos.getOutpostClimbAuto());
+    autoChooser.addOption("Score in Center", autos.getCenterScoreAuto());
     autoChooser.addOption("Test Auto", autos.getTestAuto());
     autoChooser.addOption("Just Score", autos.getJustScoreAuto());
 

From e5482fad14d2cb6756b03e7af62ffaf4a4ea189f Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 13 Mar 2026 23:42:07 -0700
Subject: [PATCH 12/54] face left for center score auto

---
 src/main/deploy/choreo/PMtoM.traj         | 7 ++++---
 src/main/deploy/choreo/rebuiltChoreo.chor | 8 ++++----
 2 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/deploy/choreo/PMtoM.traj b/src/main/deploy/choreo/PMtoM.traj
index 2a744b63..8c342089 100644
--- a/src/main/deploy/choreo/PMtoM.traj
+++ b/src/main/deploy/choreo/PMtoM.traj
@@ -14,13 +14,14 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"PM.x", "val":3.674594640731811}, "y":{"exp":"PM.y", "val":4.0000072192}, "heading":{"exp":"PM.heading", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"M.x", "val":1.8712037801742551}, "y":{"exp":"M.y", "val":4.0}, "heading":{"exp":"M.heading", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"PM.x", "val":3.674594640731811}, "y":{"exp":"PM.y", "val":4.0000072192}, "heading":{"exp":"PM.heading", "val":1.5707963192945}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"M.x", "val":1.8712037801742551}, "y":{"exp":"M.y", "val":4.0}, "heading":{"exp":"M.heading", "val":1.570796319299}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
-    {"from":1, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}],
+    {"from":1, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+    {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0 m / s", "val":0.0}}}, "enabled":true}],
   "targetDt":{
    "exp":"0.05 s",
    "val":0.05
diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor
index 4a4bcd90..a081ffec 100644
--- a/src/main/deploy/choreo/rebuiltChoreo.chor
+++ b/src/main/deploy/choreo/rebuiltChoreo.chor
@@ -240,8 +240,8 @@
      "val":4.0
     },
     "heading":{
-     "exp":"0 rad",
-     "val":0.0
+     "exp":"3.141592638598 / 2 rad",
+     "val":1.570796319299
     }
    },
    "MLL":{
@@ -324,8 +324,8 @@
      "val":4.0000072192
     },
     "heading":{
-     "exp":"0 rad",
-     "val":0.0
+     "exp":"3.141592638589 / 2 rad",
+     "val":1.5707963192945
     }
    },
    "PRB":{

From ddeaf64f059d4e040333a9b02ff5bea8a2db2c31 Mon Sep 17 00:00:00 2001
From: vivi-o <vivie@oesterer.com>
Date: Fri, 13 Mar 2026 23:45:21 -0700
Subject: [PATCH 13/54] adjust path

---
 src/main/deploy/choreo/PMtoM.traj | 49 +++++++++++++++----------------
 1 file changed, 24 insertions(+), 25 deletions(-)

diff --git a/src/main/deploy/choreo/PMtoM.traj b/src/main/deploy/choreo/PMtoM.traj
index 8c342089..67f4ab35 100644
--- a/src/main/deploy/choreo/PMtoM.traj
+++ b/src/main/deploy/choreo/PMtoM.traj
@@ -3,8 +3,8 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":3.674594640731811, "y":4.0000072192, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.8712037801742551, "y":4.0, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":3.674594640731811, "y":4.0000072192, "heading":1.5707963192945, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.8712037801742551, "y":4.0, "heading":1.570796319299, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -20,8 +20,7 @@
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
-    {"from":1, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
-    {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0 m / s", "val":0.0}}}, "enabled":true}],
+    {"from":1, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}],
   "targetDt":{
    "exp":"0.05 s",
    "val":0.05
@@ -54,27 +53,27 @@
   "sampleType":"Swerve",
   "waypoints":[0.0,1.95293],
   "samples":[
-    {"t":0.0, "x":3.67459, "y":4.00001, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52887, "ay":-0.00003, "alpha":0.0, "fx":[-124.47811,-124.47811,-124.47811,-124.47811], "fy":[-0.0005,-0.0005,-0.0005,-0.0005]},
-    {"t":0.09765, "x":3.6387, "y":4.00001, "heading":0.0, "vx":-0.73517, "vy":0.0, "omega":0.0, "ax":-2.71112, "ay":-0.00001, "alpha":0.0, "fx":[-44.82422,-44.82422,-44.82422,-44.82422], "fy":[-0.00018,-0.00018,-0.00018,-0.00018]},
-    {"t":0.19529, "x":3.55399, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.29294, "x":3.45635, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.39059, "x":3.35872, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.48823, "x":3.26108, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.58588, "x":3.16344, "y":4.00001, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.68352, "x":3.06581, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.78117, "x":2.96817, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.87882, "x":2.87054, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.97646, "x":2.7729, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.07411, "x":2.67526, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.17176, "x":2.57763, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.2694, "x":2.47999, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.36705, "x":2.38235, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.4647, "x":2.28472, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.56234, "x":2.18708, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.65999, "x":2.08944, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.75763, "x":1.99181, "y":4.0, "heading":0.0, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":2.71112, "ay":0.00001, "alpha":0.0, "fx":[44.82422,44.82422,44.82422,44.82422], "fy":[0.00018,0.00018,0.00018,0.00018]},
-    {"t":1.85528, "x":1.9071, "y":4.0, "heading":0.0, "vx":-0.73517, "vy":0.0, "omega":0.0, "ax":7.52887, "ay":0.00003, "alpha":0.0, "fx":[124.47811,124.47811,124.47811,124.47811], "fy":[0.0005,0.0005,0.0005,0.0005]},
-    {"t":1.95293, "x":1.8712, "y":4.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":3.67459, "y":4.00001, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52887, "ay":-0.00003, "alpha":0.0, "fx":[-124.47812,-124.47812,-124.47812,-124.47812], "fy":[-0.0005,-0.0005,-0.0005,-0.0005]},
+    {"t":0.09765, "x":3.6387, "y":4.00001, "heading":1.5708, "vx":-0.73517, "vy":0.0, "omega":0.0, "ax":-2.71112, "ay":-0.00001, "alpha":0.0, "fx":[-44.82422,-44.82422,-44.82422,-44.82422], "fy":[-0.00018,-0.00018,-0.00018,-0.00018]},
+    {"t":0.19529, "x":3.55399, "y":4.00001, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.29294, "x":3.45635, "y":4.00001, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39059, "x":3.35872, "y":4.00001, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.48823, "x":3.26108, "y":4.00001, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.58588, "x":3.16344, "y":4.00001, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.68352, "x":3.06581, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.78117, "x":2.96817, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.87882, "x":2.87054, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.97646, "x":2.7729, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.07411, "x":2.67526, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.17176, "x":2.57763, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.2694, "x":2.47999, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.36705, "x":2.38235, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.4647, "x":2.28472, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.56234, "x":2.18708, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.65999, "x":2.08944, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.75763, "x":1.99181, "y":4.0, "heading":1.5708, "vx":-0.9999, "vy":0.0, "omega":0.0, "ax":2.71112, "ay":0.00001, "alpha":0.0, "fx":[44.82422,44.82422,44.82422,44.82422], "fy":[0.00018,0.00018,0.00018,0.00018]},
+    {"t":1.85528, "x":1.9071, "y":4.0, "heading":1.5708, "vx":-0.73517, "vy":0.0, "omega":0.0, "ax":7.52887, "ay":0.00003, "alpha":0.0, "fx":[124.47812,124.47812,124.47812,124.47812], "fy":[0.0005,0.0005,0.0005,0.0005]},
+    {"t":1.95293, "x":1.8712, "y":4.0, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]

From 834bc8eec8926a4f45c74969b8d00167b0274a7f Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 07:17:26 -0700
Subject: [PATCH 14/54] add custom map from yesterday

---
 .../tagmaps/field_map_mar_13_18_10_19.json    | 585 ++++++++++++++++++
 .../swerve/constants/SwerveConstants.java     |  29 +-
 2 files changed, 601 insertions(+), 13 deletions(-)
 create mode 100644 src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json

diff --git a/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json b/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json
new file mode 100644
index 00000000..53eb4e72
--- /dev/null
+++ b/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json
@@ -0,0 +1,585 @@
+{
+  "tags" : [
+    {
+      "ID" : 1,
+      "pose" : {
+        "translation" : {
+          "x" : 12.050434112548828,
+          "y" : 6.9188642501831055,
+          "z" : 1.1512539386749268
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.0319787934422493,
+            "X" : -0.0173358041793108,
+            "Y" : 0.0013840891188010573,
+            "Z" : 0.9993372559547424
+          }
+        }
+      }
+    },
+    {
+      "ID" : 2,
+      "pose" : {
+        "translation" : {
+          "x" : 11.923489570617676,
+          "y" : 4.128251552581787,
+          "z" : 1.3825604915618896
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.7207949161529541,
+            "X" : -0.015484735369682312,
+            "Y" : -0.0014652509707957506,
+            "Z" : 0.6929739117622375
+          }
+        }
+      }
+    },
+    {
+      "ID" : 3,
+      "pose" : {
+        "translation" : {
+          "x" : 11.331823348999023,
+          "y" : 3.782963275909424,
+          "z" : 1.3673444986343384
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.01351415179669857,
+            "X" : -0.021491853520274162,
+            "Y" : 0.004888125695288181,
+            "Z" : 0.9996656775474548
+          }
+        }
+      }
+    },
+    {
+      "ID" : 4,
+      "pose" : {
+        "translation" : {
+          "x" : 11.300000190734863,
+          "y" : 3.435410976409912,
+          "z" : 1.371805191040039
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.02158663608133793,
+            "X" : -0.022888774052262306,
+            "Y" : 0.004296557977795601,
+            "Z" : 0.9994957447052002
+          }
+        }
+      }
+    },
+    {
+      "ID" : 5,
+      "pose" : {
+        "translation" : {
+          "x" : 11.903764724731445,
+          "y" : 2.7956435680389404,
+          "z" : 1.383049488067627
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.6885595917701721,
+            "X" : 0.016876405104994774,
+            "Y" : -0.004065195098519325,
+            "Z" : -0.7249720096588135
+          }
+        }
+      }
+    },
+    {
+      "ID" : 6,
+      "pose" : {
+        "translation" : {
+          "x" : 11.706083297729492,
+          "y" : 0.14464473724365234,
+          "z" : 1.157674789428711
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.025160714983940125,
+            "X" : -0.019202761352062225,
+            "Y" : 0.002425266895443201,
+            "Z" : 0.9994960427284241
+          }
+        }
+      }
+    },
+    {
+      "ID" : 7,
+      "pose" : {
+        "translation" : {
+          "x" : 11.779255867004395,
+          "y" : 0.1305246353149414,
+          "z" : 1.1589089632034302
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9994612336158752,
+            "X" : -0.0020718644373118877,
+            "Y" : -0.01980898156762123,
+            "Z" : -0.02608805149793625
+          }
+        }
+      }
+    },
+    {
+      "ID" : 8,
+      "pose" : {
+        "translation" : {
+          "x" : 12.259369850158691,
+          "y" : 2.77305269241333,
+          "z" : 1.3927721977233887
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.6856842041015625,
+            "X" : 0.019558781757950783,
+            "Y" : -0.001896911533549428,
+            "Z" : -0.7276337742805481
+          }
+        }
+      }
+    },
+    {
+      "ID" : 9,
+      "pose" : {
+        "translation" : {
+          "x" : 12.518342018127441,
+          "y" : 3.0062994956970215,
+          "z" : 1.4028586149215698
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9991946220397949,
+            "X" : 0.0011942394776269794,
+            "Y" : -0.01824851520359516,
+            "Z" : -0.03571748360991478
+          }
+        }
+      }
+    },
+    {
+      "ID" : 10,
+      "pose" : {
+        "translation" : {
+          "x" : 12.526363372802734,
+          "y" : 3.5018177032470703,
+          "z" : 1.410685658454895
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.999525785446167,
+            "X" : -0.0003090753161814064,
+            "Y" : -0.007112081162631512,
+            "Z" : -0.0299605093896389
+          }
+        }
+      }
+    },
+    {
+      "ID" : 11,
+      "pose" : {
+        "translation" : {
+          "x" : 12.401211738586426,
+          "y" : 4.053260326385498,
+          "z" : 1.4090445041656494
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.7207784652709961,
+            "X" : -0.015011371113359928,
+            "Y" : -0.010919488966464996,
+            "Z" : 0.6929170489311218
+          }
+        }
+      }
+    },
+    {
+      "ID" : 12,
+      "pose" : {
+        "translation" : {
+          "x" : 12.1367826461792,
+          "y" : 6.854067802429199,
+          "z" : 1.1314808130264282
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9998978972434998,
+            "X" : 0.0032891076989471912,
+            "Y" : 0.0008480203105136752,
+            "Z" : -0.013879206962883472
+          }
+        }
+      }
+    },
+    {
+      "ID" : 13,
+      "pose" : {
+        "translation" : {
+          "x" : 16.69558334350586,
+          "y" : 6.610563278198242,
+          "z" : 0.9037705659866333
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.0234692245721817,
+            "X" : -0.002002944238483906,
+            "Y" : -0.0003050622472073883,
+            "Z" : 0.9997224807739258
+          }
+        }
+      }
+    },
+    {
+      "ID" : 14,
+      "pose" : {
+        "translation" : {
+          "x" : 16.67595863342285,
+          "y" : 6.18171501159668,
+          "z" : 0.9039492607116699
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.010960479266941547,
+            "X" : 0.0005166280898265541,
+            "Y" : -0.002506508957594633,
+            "Z" : 0.999936580657959
+          }
+        }
+      }
+    },
+    {
+      "ID" : 15,
+      "pose" : {
+        "translation" : {
+          "x" : 16.564498901367188,
+          "y" : 3.5477375984191895,
+          "z" : 0.9101831316947937
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.029302222654223442,
+            "X" : 0.003682132810354233,
+            "Y" : 0.0026310565881431103,
+            "Z" : 0.9995603561401367
+          }
+        }
+      }
+    },
+    {
+      "ID" : 16,
+      "pose" : {
+        "translation" : {
+          "x" : 16.539405822753906,
+          "y" : 3.1178951263427734,
+          "z" : 0.9153234958648682
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.007331775967031717,
+            "X" : -0.008358882740139961,
+            "Y" : 0.00232889992184937,
+            "Z" : 0.9999354481697083
+          }
+        }
+      }
+    },
+    {
+      "ID" : 17,
+      "pose" : {
+        "translation" : {
+          "x" : 4.522510051727295,
+          "y" : 0.4364204406738281,
+          "z" : 0.9622411131858826
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9995527267456055,
+            "X" : -0.0019414271228015423,
+            "Y" : -0.010864048264920712,
+            "Z" : -0.027793319895863533
+          }
+        }
+      }
+    },
+    {
+      "ID" : 18,
+      "pose" : {
+        "translation" : {
+          "x" : 4.58988618850708,
+          "y" : 3.195493221282959,
+          "z" : 1.1940523386001587
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.691650390625,
+            "X" : 0.01303744874894619,
+            "Y" : -0.00376962311565876,
+            "Z" : -0.7221049666404724
+          }
+        }
+      }
+    },
+    {
+      "ID" : 19,
+      "pose" : {
+        "translation" : {
+          "x" : 5.226935386657715,
+          "y" : 3.45327091217041,
+          "z" : 1.2011194229125977
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9997949600219727,
+            "X" : 0.0005262811318971217,
+            "Y" : -0.010933312587440014,
+            "Z" : -0.01703987829387188
+          }
+        }
+      }
+    },
+    {
+      "ID" : 20,
+      "pose" : {
+        "translation" : {
+          "x" : 5.234817981719971,
+          "y" : 3.8026344776153564,
+          "z" : 1.2003945112228394
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9998764991760254,
+            "X" : 0.00011397890921216458,
+            "Y" : -0.008693280629813671,
+            "Z" : -0.013094102963805199
+          }
+        }
+      }
+    },
+    {
+      "ID" : 21,
+      "pose" : {
+        "translation" : {
+          "x" : 4.664792537689209,
+          "y" : 4.426998615264893,
+          "z" : 1.1946265697479248
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.7123200297355652,
+            "X" : -0.002563168527558446,
+            "Y" : -0.011971132829785347,
+            "Z" : 0.7017481327056885
+          }
+        }
+      }
+    },
+    {
+      "ID" : 22,
+      "pose" : {
+        "translation" : {
+          "x" : 4.825745105743408,
+          "y" : 7.208614349365234,
+          "z" : 0.9605111479759216
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9997665882110596,
+            "X" : -0.0008584200986661017,
+            "Y" : -0.021474771201610565,
+            "Z" : -0.002250437159091234
+          }
+        }
+      }
+    },
+    {
+      "ID" : 23,
+      "pose" : {
+        "translation" : {
+          "x" : 4.734621047973633,
+          "y" : 7.202859878540039,
+          "z" : 0.9770094156265259
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.034675028175115585,
+            "X" : -0.018650785088539124,
+            "Y" : 0.0029087455477565527,
+            "Z" : 0.9992203116416931
+          }
+        }
+      }
+    },
+    {
+      "ID" : 24,
+      "pose" : {
+        "translation" : {
+          "x" : 4.310214519500732,
+          "y" : 4.4429521560668945,
+          "z" : 1.1934773921966553
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.7160341739654541,
+            "X" : -0.011152027174830437,
+            "Y" : -0.0041417754255235195,
+            "Z" : 0.6979638934135437
+          }
+        }
+      }
+    },
+    {
+      "ID" : 25,
+      "pose" : {
+        "translation" : {
+          "x" : 4.0436787605285645,
+          "y" : 4.20996618270874,
+          "z" : 1.1983577013015747
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.01692536473274231,
+            "X" : -0.02695719338953495,
+            "Y" : -0.0008292849524877965,
+            "Z" : 0.9994929432868958
+          }
+        }
+      }
+    },
+    {
+      "ID" : 26,
+      "pose" : {
+        "translation" : {
+          "x" : 4.030979633331299,
+          "y" : 3.853869915008545,
+          "z" : 1.1997509002685547
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.017164066433906555,
+            "X" : -0.031823523342609406,
+            "Y" : 0.001335379434749484,
+            "Z" : 0.9993451833724976
+          }
+        }
+      }
+    },
+    {
+      "ID" : 27,
+      "pose" : {
+        "translation" : {
+          "x" : 4.239171028137207,
+          "y" : 3.2177231311798096,
+          "z" : 1.1884853839874268
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.6884703040122986,
+            "X" : 0.014258901588618755,
+            "Y" : 0.0001352958206553012,
+            "Z" : -0.7251242399215698
+          }
+        }
+      }
+    },
+    {
+      "ID" : 28,
+      "pose" : {
+        "translation" : {
+          "x" : 4.380957126617432,
+          "y" : 0.46189260482788086,
+          "z" : 0.9888477325439453
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.018142135813832283,
+            "X" : -0.009404327720403671,
+            "Y" : 0.0032862841617316008,
+            "Z" : 0.9997857809066772
+          }
+        }
+      }
+    },
+    {
+      "ID" : 29,
+      "pose" : {
+        "translation" : {
+          "x" : -0.17474985122680664,
+          "y" : 0.7096297740936279,
+          "z" : 0.5477728247642517
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9998412728309631,
+            "X" : -0.00282590021379292,
+            "Y" : -0.006864454131573439,
+            "Z" : -0.01619541272521019
+          }
+        }
+      }
+    },
+    {
+      "ID" : 30,
+      "pose" : {
+        "translation" : {
+          "x" : -0.15256865322589874,
+          "y" : 1.1327531337738037,
+          "z" : 0.548723578453064
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9996415376663208,
+            "X" : -0.002256404608488083,
+            "Y" : -0.012892674654722214,
+            "Z" : -0.0233586598187685
+          }
+        }
+      }
+    },
+    {
+      "ID" : 31,
+      "pose" : {
+        "translation" : {
+          "x" : -0.006732862442731857,
+          "y" : 3.744476795196533,
+          "z" : 0.5510860085487366
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 0.9999865889549255,
+            "X" : -0.001071035978384316,
+            "Y" : -0.0008199909934774041,
+            "Z" : -0.004985099658370018
+          }
+        }
+      }
+    },
+    {
+      "ID" : 32,
+      "pose" : {
+        "translation" : {
+          "x" : 0.008077199570834637,
+          "y" : 4.1775126457214355,
+          "z" : 0.5524500012397766
+        },
+        "rotation" : {
+          "quaternion" : {
+            "W" : 1.0,
+            "X" : 0.0,
+            "Y" : 0.0,
+            "Z" : 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field" : {
+    "length" : 16.541,
+    "width" : 8.069
+  },
+  "_comment" : "generated by PractiCal 1.0.5 (3) on 3/13/2026"
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java
index e854fd6a..5ca16bde 100644
--- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java
+++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.swerve.constants;
 
+import java.io.File;
+
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -12,6 +14,7 @@
 import edu.wpi.first.units.measure.Mass;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.Filesystem;
 import frc.robot.components.camera.Camera.CameraConstants;
 import frc.robot.subsystems.swerve.module.Module.ModuleConstants;
 
@@ -24,19 +27,19 @@ public abstract class SwerveConstants {
   protected AprilTagFieldLayout fieldTags;
 
   public SwerveConstants() {
-    // try {
-    //   fieldTags =
-    //       new AprilTagFieldLayout(
-    //           Filesystem.getDeployDirectory()
-    //               .toPath()
-    //               .resolve("tagmaps" + File.separator + "2026-rebuilt-welded.json"));
-    //   System.out.println("Successfully loaded tag map");
-    // } catch (Exception e) {
-    //   System.err.println("Failed to load custom tag map");
-    //   tagLoadFailureAlert.set(true);
-    //   fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-    // }
-    fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+    try {
+      fieldTags =
+          new AprilTagFieldLayout(
+              Filesystem.getDeployDirectory()
+                  .toPath()
+                  .resolve("tagmaps" + File.separator + "field_map_mar_13_18_10_19.json"));
+      System.out.println("Successfully loaded tag map");
+    } catch (Exception e) {
+      System.err.println("Failed to load custom tag map");
+      tagLoadFailureAlert.set(true);
+      fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+    }
+    // fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
   }
 
   public AprilTagFieldLayout getFieldTagLayout() {

From f1c92ac448a6d4cd9258970338dcd7fa0f75ef30 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 07:28:10 -0700
Subject: [PATCH 15/54] turn off tuning mode

---
 src/main/java/frc/robot/Robot.java                 | 2 +-
 src/main/java/frc/robot/utils/autoaim/AutoAim.java | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1d8fd8a4..ab411dfa 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -144,7 +144,7 @@ public enum RobotEdition {
    * This is for when we're testing shot and extension numbers and should be FALSE once bring up is
    * complete
    */
-  public static final boolean TUNING_MODE = true;
+  public static final boolean TUNING_MODE = false;
 
   public boolean hasZeroedSinceStartup = false;
 
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 6a12c504..103975b1 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -19,7 +19,8 @@ public class AutoAim {
   private static boolean outOfRange = false; // TODO not sure if this should be true by default
 
   public static double LATENCY_COMPENSATION_SECS =
-      new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp
+    //   new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp
+    0.3;
   //   public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time
 
   public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree();

From 6547cef76b236e7717421184c470ba9845d04192 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 07:43:24 -0700
Subject: [PATCH 16/54] turn off tuning mode

---
 src/main/java/frc/robot/Robot.java                 | 5 +++--
 src/main/java/frc/robot/utils/autoaim/AutoAim.java | 3 ++-
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f5c3cfde..de9315a6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -144,7 +144,7 @@ public enum RobotEdition {
    * This is for when we're testing shot and extension numbers and should be FALSE once bring up is
    * complete
    */
-  public static final boolean TUNING_MODE = true;
+  public static final boolean TUNING_MODE = false;
 
   public boolean hasZeroedSinceStartup = false;
 
@@ -426,7 +426,7 @@ public Robot() {
     superstructure =
         new Superstructure(swerve, indexer, intake, shooter, climber, driver, operator);
 
-    DriverStation.silenceJoystickConnectionWarning(true);
+    DriverStation.silenceJoystickConnectionWarning(false);
     SignalLogger.enableAutoLogging(false);
     RobotController.setBrownoutVoltage(6.0);
 
@@ -709,6 +709,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
 
     new Trigger(() -> DriverStation.isJoystickConnected(1))
         .negate()
+        .or(() -> DriverStation.getStickButton(1, 3))
         .onTrue(Commands.runOnce(() -> operatorJoystickDisconnectedAlert.set(true)))
         .onFalse(Commands.runOnce(() -> operatorJoystickDisconnectedAlert.set(false)));
   }
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 6a12c504..103975b1 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -19,7 +19,8 @@ public class AutoAim {
   private static boolean outOfRange = false; // TODO not sure if this should be true by default
 
   public static double LATENCY_COMPENSATION_SECS =
-      new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp
+    //   new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp
+    0.3;
   //   public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time
 
   public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree();

From 5b923bca8427228e10126c370f857e9fd8b358b4 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:23:44 -0700
Subject: [PATCH 17/54] not limit scoring to on shift bc i'm scared

---
 src/main/java/frc/robot/Superstructure.java | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index abfa896d..f9f350a6 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -793,8 +793,10 @@ public boolean inScoringArea() {
   }
 
   public boolean canScore() {
-    return (isOurShift() || !DriverStation.isFMSAttached())
-        && (inScoringArea() || poseOverride)
+    return 
+    // (isOurShift() || !DriverStation.isFMSAttached())
+    //     && 
+        (inScoringArea() || poseOverride)
         && (!swerve.isNearTrench() || poseOverride || fixedShotTarget != FixedShotTarget.NONE);
   }
 

From 3ec61abfe319b49c1b35d71c98551e679ebe4fd7 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:45:44 -0700
Subject: [PATCH 18/54] Update at 'Sat Mar 14 08:45:44 PDT 2026'

---
 .../subsystems/shooter/TurretSubsystem.java   |  2 +-
 .../java/frc/robot/utils/autoaim/AutoAim.java | 70 +++++++++++--------
 2 files changed, 43 insertions(+), 29 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
index 5d190b81..e479e2d8 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
@@ -38,7 +38,7 @@
 public class TurretSubsystem extends SubsystemBase implements Shooter {
 
   /** Creates a new TurretSubsystem. */
-  public static final double HOOD_GEAR_RATIO = 58.96875;
+  public static final double HOOD_GEAR_RATIO = 33.8671875;//58.96875;
 
   public static final double FLYWHEEL_GEAR_RATIO = 0.84615384615;
 
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 103975b1..eac52efd 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -66,30 +66,44 @@ public class AutoAim {
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12),
-        new ShotData(Rotation2d.fromDegrees(25), 35 - 6, 1.14));
+        new ShotData(Rotation2d.fromDegrees(25), 35 
+        // - 6
+        , 1.14));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12),
-        new ShotData(Rotation2d.fromDegrees(26), 37 - 6, 1.10));
+        new ShotData(Rotation2d.fromDegrees(26), 37 
+        // - 6
+        , 1.10));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12),
-        new ShotData(Rotation2d.fromDegrees(30), 37 - 6, 1.09));
+        new ShotData(Rotation2d.fromDegrees(30), 37 
+        // - 6
+        , 1.09));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12),
-        new ShotData(Rotation2d.fromDegrees(33), 37 - 6, 1.15));
+        new ShotData(Rotation2d.fromDegrees(33), 37 
+        // - 6
+        , 1.15));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12),
-        new ShotData(Rotation2d.fromDegrees(36), 38 - 6, 1.23));
+        new ShotData(Rotation2d.fromDegrees(36), 38 
+        // - 6
+        , 1.23));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12),
-        new ShotData(Rotation2d.fromDegrees(38), 39 - 6, 1.33));
+        new ShotData(Rotation2d.fromDegrees(38), 39 
+        // - 6
+        , 1.33));
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12),
-        new ShotData(Rotation2d.fromDegrees(39), 40.5 - 6, 1.35));
+        new ShotData(Rotation2d.fromDegrees(39), 40.5 
+        // - 6
+        , 1.35));
   }
 
   // Ig we'll see if we need more than 1 feed shot tree
@@ -97,49 +111,49 @@ public class AutoAim {
 
   static { // For feed shot tree
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0));
+        Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 , 0)); // - 2, 0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0));
+        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 , 0.0)); //- 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0));
+        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30, 0.0)); //- 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0));
+        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32, 0.0)); //- 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0));
+        Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0));
+        Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0));
+        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38, 0.0));// - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0));
+        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40, 0.0));// - 2, 0.0));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42));
+        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38, 1.42)); //- 2, 1.42));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36));
+        Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40, 1.36)); // - 2, 1.36));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34));
+        Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41, 1.34)); // - 2, 1.34));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25));
+        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 , 1.25)); //- 2, 1.25));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28));
+        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43, 1.28));// - 2, 1.28));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27));
+        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45, 1.27));// - 2, 1.27));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32));
+        Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46, 1.32)); // - 2, 1.32));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4));
+        Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48, 1.4)); // - 2, 1.4));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3));
+        Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33));
+        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53, 1.33));//- 2, 1.33));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3));
+        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57, 1.3));// - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2));
+        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57, 1.2));// - 2, 1.2));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2));
+        Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59, 1.2)); // - 2, 1.2));
 
     // TODO: POPULATE beyond 24 feet and time of flight
   }

From fff77760dcbc1c13255beeb26404f97d987e0d50 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:56:26 -0700
Subject: [PATCH 19/54] Update at 'Sat Mar 14 08:56:26 PDT 2026'

---
 .../tagmaps/field_map_mar_13_18_10_19.json    | 780 +++++++++---------
 src/main/java/frc/robot/Robot.java            |   3 +-
 src/main/java/frc/robot/Superstructure.java   |   6 +-
 .../subsystems/shooter/TurretSubsystem.java   |   2 +-
 .../swerve/constants/SwerveConstants.java     |   3 +-
 .../java/frc/robot/utils/autoaim/AutoAim.java |  95 ++-
 6 files changed, 455 insertions(+), 434 deletions(-)

diff --git a/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json b/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json
index 53eb4e72..4c24c2e9 100644
--- a/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json
+++ b/src/main/deploy/tagmaps/field_map_mar_13_18_10_19.json
@@ -1,585 +1,585 @@
 {
-  "tags" : [
+  "tags": [
     {
-      "ID" : 1,
-      "pose" : {
-        "translation" : {
-          "x" : 12.050434112548828,
-          "y" : 6.9188642501831055,
-          "z" : 1.1512539386749268
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 12.050434112548828,
+          "y": 6.9188642501831055,
+          "z": 1.1512539386749268
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.0319787934422493,
-            "X" : -0.0173358041793108,
-            "Y" : 0.0013840891188010573,
-            "Z" : 0.9993372559547424
+        "rotation": {
+          "quaternion": {
+            "W": 0.0319787934422493,
+            "X": -0.0173358041793108,
+            "Y": 0.0013840891188010573,
+            "Z": 0.9993372559547424
           }
         }
       }
     },
     {
-      "ID" : 2,
-      "pose" : {
-        "translation" : {
-          "x" : 11.923489570617676,
-          "y" : 4.128251552581787,
-          "z" : 1.3825604915618896
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 11.923489570617676,
+          "y": 4.128251552581787,
+          "z": 1.3825604915618896
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.7207949161529541,
-            "X" : -0.015484735369682312,
-            "Y" : -0.0014652509707957506,
-            "Z" : 0.6929739117622375
+        "rotation": {
+          "quaternion": {
+            "W": 0.7207949161529541,
+            "X": -0.015484735369682312,
+            "Y": -0.0014652509707957506,
+            "Z": 0.6929739117622375
           }
         }
       }
     },
     {
-      "ID" : 3,
-      "pose" : {
-        "translation" : {
-          "x" : 11.331823348999023,
-          "y" : 3.782963275909424,
-          "z" : 1.3673444986343384
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.331823348999023,
+          "y": 3.782963275909424,
+          "z": 1.3673444986343384
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.01351415179669857,
-            "X" : -0.021491853520274162,
-            "Y" : 0.004888125695288181,
-            "Z" : 0.9996656775474548
+        "rotation": {
+          "quaternion": {
+            "W": 0.01351415179669857,
+            "X": -0.021491853520274162,
+            "Y": 0.004888125695288181,
+            "Z": 0.9996656775474548
           }
         }
       }
     },
     {
-      "ID" : 4,
-      "pose" : {
-        "translation" : {
-          "x" : 11.300000190734863,
-          "y" : 3.435410976409912,
-          "z" : 1.371805191040039
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 11.300000190734863,
+          "y": 3.435410976409912,
+          "z": 1.371805191040039
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.02158663608133793,
-            "X" : -0.022888774052262306,
-            "Y" : 0.004296557977795601,
-            "Z" : 0.9994957447052002
+        "rotation": {
+          "quaternion": {
+            "W": 0.02158663608133793,
+            "X": -0.022888774052262306,
+            "Y": 0.004296557977795601,
+            "Z": 0.9994957447052002
           }
         }
       }
     },
     {
-      "ID" : 5,
-      "pose" : {
-        "translation" : {
-          "x" : 11.903764724731445,
-          "y" : 2.7956435680389404,
-          "z" : 1.383049488067627
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 11.903764724731445,
+          "y": 2.7956435680389404,
+          "z": 1.383049488067627
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.6885595917701721,
-            "X" : 0.016876405104994774,
-            "Y" : -0.004065195098519325,
-            "Z" : -0.7249720096588135
+        "rotation": {
+          "quaternion": {
+            "W": 0.6885595917701721,
+            "X": 0.016876405104994774,
+            "Y": -0.004065195098519325,
+            "Z": -0.7249720096588135
           }
         }
       }
     },
     {
-      "ID" : 6,
-      "pose" : {
-        "translation" : {
-          "x" : 11.706083297729492,
-          "y" : 0.14464473724365234,
-          "z" : 1.157674789428711
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 11.706083297729492,
+          "y": 0.14464473724365234,
+          "z": 1.157674789428711
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.025160714983940125,
-            "X" : -0.019202761352062225,
-            "Y" : 0.002425266895443201,
-            "Z" : 0.9994960427284241
+        "rotation": {
+          "quaternion": {
+            "W": 0.025160714983940125,
+            "X": -0.019202761352062225,
+            "Y": 0.002425266895443201,
+            "Z": 0.9994960427284241
           }
         }
       }
     },
     {
-      "ID" : 7,
-      "pose" : {
-        "translation" : {
-          "x" : 11.779255867004395,
-          "y" : 0.1305246353149414,
-          "z" : 1.1589089632034302
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 11.779255867004395,
+          "y": 0.1305246353149414,
+          "z": 1.1589089632034302
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9994612336158752,
-            "X" : -0.0020718644373118877,
-            "Y" : -0.01980898156762123,
-            "Z" : -0.02608805149793625
+        "rotation": {
+          "quaternion": {
+            "W": 0.9994612336158752,
+            "X": -0.0020718644373118877,
+            "Y": -0.01980898156762123,
+            "Z": -0.02608805149793625
           }
         }
       }
     },
     {
-      "ID" : 8,
-      "pose" : {
-        "translation" : {
-          "x" : 12.259369850158691,
-          "y" : 2.77305269241333,
-          "z" : 1.3927721977233887
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 12.259369850158691,
+          "y": 2.77305269241333,
+          "z": 1.3927721977233887
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.6856842041015625,
-            "X" : 0.019558781757950783,
-            "Y" : -0.001896911533549428,
-            "Z" : -0.7276337742805481
+        "rotation": {
+          "quaternion": {
+            "W": 0.6856842041015625,
+            "X": 0.019558781757950783,
+            "Y": -0.001896911533549428,
+            "Z": -0.7276337742805481
           }
         }
       }
     },
     {
-      "ID" : 9,
-      "pose" : {
-        "translation" : {
-          "x" : 12.518342018127441,
-          "y" : 3.0062994956970215,
-          "z" : 1.4028586149215698
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.518342018127441,
+          "y": 3.0062994956970215,
+          "z": 1.4028586149215698
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9991946220397949,
-            "X" : 0.0011942394776269794,
-            "Y" : -0.01824851520359516,
-            "Z" : -0.03571748360991478
+        "rotation": {
+          "quaternion": {
+            "W": 0.9991946220397949,
+            "X": 0.0011942394776269794,
+            "Y": -0.01824851520359516,
+            "Z": -0.03571748360991478
           }
         }
       }
     },
     {
-      "ID" : 10,
-      "pose" : {
-        "translation" : {
-          "x" : 12.526363372802734,
-          "y" : 3.5018177032470703,
-          "z" : 1.410685658454895
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.526363372802734,
+          "y": 3.5018177032470703,
+          "z": 1.410685658454895
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.999525785446167,
-            "X" : -0.0003090753161814064,
-            "Y" : -0.007112081162631512,
-            "Z" : -0.0299605093896389
+        "rotation": {
+          "quaternion": {
+            "W": 0.999525785446167,
+            "X": -0.0003090753161814064,
+            "Y": -0.007112081162631512,
+            "Z": -0.0299605093896389
           }
         }
       }
     },
     {
-      "ID" : 11,
-      "pose" : {
-        "translation" : {
-          "x" : 12.401211738586426,
-          "y" : 4.053260326385498,
-          "z" : 1.4090445041656494
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.401211738586426,
+          "y": 4.053260326385498,
+          "z": 1.4090445041656494
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.7207784652709961,
-            "X" : -0.015011371113359928,
-            "Y" : -0.010919488966464996,
-            "Z" : 0.6929170489311218
+        "rotation": {
+          "quaternion": {
+            "W": 0.7207784652709961,
+            "X": -0.015011371113359928,
+            "Y": -0.010919488966464996,
+            "Z": 0.6929170489311218
           }
         }
       }
     },
     {
-      "ID" : 12,
-      "pose" : {
-        "translation" : {
-          "x" : 12.1367826461792,
-          "y" : 6.854067802429199,
-          "z" : 1.1314808130264282
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 12.1367826461792,
+          "y": 6.854067802429199,
+          "z": 1.1314808130264282
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9998978972434998,
-            "X" : 0.0032891076989471912,
-            "Y" : 0.0008480203105136752,
-            "Z" : -0.013879206962883472
+        "rotation": {
+          "quaternion": {
+            "W": 0.9998978972434998,
+            "X": 0.0032891076989471912,
+            "Y": 0.0008480203105136752,
+            "Z": -0.013879206962883472
           }
         }
       }
     },
     {
-      "ID" : 13,
-      "pose" : {
-        "translation" : {
-          "x" : 16.69558334350586,
-          "y" : 6.610563278198242,
-          "z" : 0.9037705659866333
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.69558334350586,
+          "y": 6.610563278198242,
+          "z": 0.9037705659866333
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.0234692245721817,
-            "X" : -0.002002944238483906,
-            "Y" : -0.0003050622472073883,
-            "Z" : 0.9997224807739258
+        "rotation": {
+          "quaternion": {
+            "W": 0.0234692245721817,
+            "X": -0.002002944238483906,
+            "Y": -0.0003050622472073883,
+            "Z": 0.9997224807739258
           }
         }
       }
     },
     {
-      "ID" : 14,
-      "pose" : {
-        "translation" : {
-          "x" : 16.67595863342285,
-          "y" : 6.18171501159668,
-          "z" : 0.9039492607116699
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.67595863342285,
+          "y": 6.18171501159668,
+          "z": 0.9039492607116699
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.010960479266941547,
-            "X" : 0.0005166280898265541,
-            "Y" : -0.002506508957594633,
-            "Z" : 0.999936580657959
+        "rotation": {
+          "quaternion": {
+            "W": 0.010960479266941547,
+            "X": 0.0005166280898265541,
+            "Y": -0.002506508957594633,
+            "Z": 0.999936580657959
           }
         }
       }
     },
     {
-      "ID" : 15,
-      "pose" : {
-        "translation" : {
-          "x" : 16.564498901367188,
-          "y" : 3.5477375984191895,
-          "z" : 0.9101831316947937
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.564498901367188,
+          "y": 3.5477375984191895,
+          "z": 0.9101831316947937
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.029302222654223442,
-            "X" : 0.003682132810354233,
-            "Y" : 0.0026310565881431103,
-            "Z" : 0.9995603561401367
+        "rotation": {
+          "quaternion": {
+            "W": 0.029302222654223442,
+            "X": 0.003682132810354233,
+            "Y": 0.0026310565881431103,
+            "Z": 0.9995603561401367
           }
         }
       }
     },
     {
-      "ID" : 16,
-      "pose" : {
-        "translation" : {
-          "x" : 16.539405822753906,
-          "y" : 3.1178951263427734,
-          "z" : 0.9153234958648682
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 16.539405822753906,
+          "y": 3.1178951263427734,
+          "z": 0.9153234958648682
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.007331775967031717,
-            "X" : -0.008358882740139961,
-            "Y" : 0.00232889992184937,
-            "Z" : 0.9999354481697083
+        "rotation": {
+          "quaternion": {
+            "W": 0.007331775967031717,
+            "X": -0.008358882740139961,
+            "Y": 0.00232889992184937,
+            "Z": 0.9999354481697083
           }
         }
       }
     },
     {
-      "ID" : 17,
-      "pose" : {
-        "translation" : {
-          "x" : 4.522510051727295,
-          "y" : 0.4364204406738281,
-          "z" : 0.9622411131858826
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.522510051727295,
+          "y": 0.4364204406738281,
+          "z": 0.9622411131858826
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9995527267456055,
-            "X" : -0.0019414271228015423,
-            "Y" : -0.010864048264920712,
-            "Z" : -0.027793319895863533
+        "rotation": {
+          "quaternion": {
+            "W": 0.9995527267456055,
+            "X": -0.0019414271228015423,
+            "Y": -0.010864048264920712,
+            "Z": -0.027793319895863533
           }
         }
       }
     },
     {
-      "ID" : 18,
-      "pose" : {
-        "translation" : {
-          "x" : 4.58988618850708,
-          "y" : 3.195493221282959,
-          "z" : 1.1940523386001587
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 4.58988618850708,
+          "y": 3.195493221282959,
+          "z": 1.1940523386001587
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.691650390625,
-            "X" : 0.01303744874894619,
-            "Y" : -0.00376962311565876,
-            "Z" : -0.7221049666404724
+        "rotation": {
+          "quaternion": {
+            "W": 0.691650390625,
+            "X": 0.01303744874894619,
+            "Y": -0.00376962311565876,
+            "Z": -0.7221049666404724
           }
         }
       }
     },
     {
-      "ID" : 19,
-      "pose" : {
-        "translation" : {
-          "x" : 5.226935386657715,
-          "y" : 3.45327091217041,
-          "z" : 1.2011194229125977
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 5.226935386657715,
+          "y": 3.45327091217041,
+          "z": 1.2011194229125977
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9997949600219727,
-            "X" : 0.0005262811318971217,
-            "Y" : -0.010933312587440014,
-            "Z" : -0.01703987829387188
+        "rotation": {
+          "quaternion": {
+            "W": 0.9997949600219727,
+            "X": 0.0005262811318971217,
+            "Y": -0.010933312587440014,
+            "Z": -0.01703987829387188
           }
         }
       }
     },
     {
-      "ID" : 20,
-      "pose" : {
-        "translation" : {
-          "x" : 5.234817981719971,
-          "y" : 3.8026344776153564,
-          "z" : 1.2003945112228394
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 5.234817981719971,
+          "y": 3.8026344776153564,
+          "z": 1.2003945112228394
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9998764991760254,
-            "X" : 0.00011397890921216458,
-            "Y" : -0.008693280629813671,
-            "Z" : -0.013094102963805199
+        "rotation": {
+          "quaternion": {
+            "W": 0.9998764991760254,
+            "X": 0.00011397890921216458,
+            "Y": -0.008693280629813671,
+            "Z": -0.013094102963805199
           }
         }
       }
     },
     {
-      "ID" : 21,
-      "pose" : {
-        "translation" : {
-          "x" : 4.664792537689209,
-          "y" : 4.426998615264893,
-          "z" : 1.1946265697479248
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 4.664792537689209,
+          "y": 4.426998615264893,
+          "z": 1.1946265697479248
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.7123200297355652,
-            "X" : -0.002563168527558446,
-            "Y" : -0.011971132829785347,
-            "Z" : 0.7017481327056885
+        "rotation": {
+          "quaternion": {
+            "W": 0.7123200297355652,
+            "X": -0.002563168527558446,
+            "Y": -0.011971132829785347,
+            "Z": 0.7017481327056885
           }
         }
       }
     },
     {
-      "ID" : 22,
-      "pose" : {
-        "translation" : {
-          "x" : 4.825745105743408,
-          "y" : 7.208614349365234,
-          "z" : 0.9605111479759216
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.825745105743408,
+          "y": 7.208614349365234,
+          "z": 0.9605111479759216
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9997665882110596,
-            "X" : -0.0008584200986661017,
-            "Y" : -0.021474771201610565,
-            "Z" : -0.002250437159091234
+        "rotation": {
+          "quaternion": {
+            "W": 0.9997665882110596,
+            "X": -0.0008584200986661017,
+            "Y": -0.021474771201610565,
+            "Z": -0.002250437159091234
           }
         }
       }
     },
     {
-      "ID" : 23,
-      "pose" : {
-        "translation" : {
-          "x" : 4.734621047973633,
-          "y" : 7.202859878540039,
-          "z" : 0.9770094156265259
+      "ID": 23,
+      "pose": {
+        "translation": {
+          "x": 4.734621047973633,
+          "y": 7.202859878540039,
+          "z": 0.9770094156265259
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.034675028175115585,
-            "X" : -0.018650785088539124,
-            "Y" : 0.0029087455477565527,
-            "Z" : 0.9992203116416931
+        "rotation": {
+          "quaternion": {
+            "W": 0.034675028175115585,
+            "X": -0.018650785088539124,
+            "Y": 0.0029087455477565527,
+            "Z": 0.9992203116416931
           }
         }
       }
     },
     {
-      "ID" : 24,
-      "pose" : {
-        "translation" : {
-          "x" : 4.310214519500732,
-          "y" : 4.4429521560668945,
-          "z" : 1.1934773921966553
+      "ID": 24,
+      "pose": {
+        "translation": {
+          "x": 4.310214519500732,
+          "y": 4.4429521560668945,
+          "z": 1.1934773921966553
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.7160341739654541,
-            "X" : -0.011152027174830437,
-            "Y" : -0.0041417754255235195,
-            "Z" : 0.6979638934135437
+        "rotation": {
+          "quaternion": {
+            "W": 0.7160341739654541,
+            "X": -0.011152027174830437,
+            "Y": -0.0041417754255235195,
+            "Z": 0.6979638934135437
           }
         }
       }
     },
     {
-      "ID" : 25,
-      "pose" : {
-        "translation" : {
-          "x" : 4.0436787605285645,
-          "y" : 4.20996618270874,
-          "z" : 1.1983577013015747
+      "ID": 25,
+      "pose": {
+        "translation": {
+          "x": 4.0436787605285645,
+          "y": 4.20996618270874,
+          "z": 1.1983577013015747
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.01692536473274231,
-            "X" : -0.02695719338953495,
-            "Y" : -0.0008292849524877965,
-            "Z" : 0.9994929432868958
+        "rotation": {
+          "quaternion": {
+            "W": 0.01692536473274231,
+            "X": -0.02695719338953495,
+            "Y": -0.0008292849524877965,
+            "Z": 0.9994929432868958
           }
         }
       }
     },
     {
-      "ID" : 26,
-      "pose" : {
-        "translation" : {
-          "x" : 4.030979633331299,
-          "y" : 3.853869915008545,
-          "z" : 1.1997509002685547
+      "ID": 26,
+      "pose": {
+        "translation": {
+          "x": 4.030979633331299,
+          "y": 3.853869915008545,
+          "z": 1.1997509002685547
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.017164066433906555,
-            "X" : -0.031823523342609406,
-            "Y" : 0.001335379434749484,
-            "Z" : 0.9993451833724976
+        "rotation": {
+          "quaternion": {
+            "W": 0.017164066433906555,
+            "X": -0.031823523342609406,
+            "Y": 0.001335379434749484,
+            "Z": 0.9993451833724976
           }
         }
       }
     },
     {
-      "ID" : 27,
-      "pose" : {
-        "translation" : {
-          "x" : 4.239171028137207,
-          "y" : 3.2177231311798096,
-          "z" : 1.1884853839874268
+      "ID": 27,
+      "pose": {
+        "translation": {
+          "x": 4.239171028137207,
+          "y": 3.2177231311798096,
+          "z": 1.1884853839874268
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.6884703040122986,
-            "X" : 0.014258901588618755,
-            "Y" : 0.0001352958206553012,
-            "Z" : -0.7251242399215698
+        "rotation": {
+          "quaternion": {
+            "W": 0.6884703040122986,
+            "X": 0.014258901588618755,
+            "Y": 0.0001352958206553012,
+            "Z": -0.7251242399215698
           }
         }
       }
     },
     {
-      "ID" : 28,
-      "pose" : {
-        "translation" : {
-          "x" : 4.380957126617432,
-          "y" : 0.46189260482788086,
-          "z" : 0.9888477325439453
+      "ID": 28,
+      "pose": {
+        "translation": {
+          "x": 4.380957126617432,
+          "y": 0.46189260482788086,
+          "z": 0.9888477325439453
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.018142135813832283,
-            "X" : -0.009404327720403671,
-            "Y" : 0.0032862841617316008,
-            "Z" : 0.9997857809066772
+        "rotation": {
+          "quaternion": {
+            "W": 0.018142135813832283,
+            "X": -0.009404327720403671,
+            "Y": 0.0032862841617316008,
+            "Z": 0.9997857809066772
           }
         }
       }
     },
     {
-      "ID" : 29,
-      "pose" : {
-        "translation" : {
-          "x" : -0.17474985122680664,
-          "y" : 0.7096297740936279,
-          "z" : 0.5477728247642517
+      "ID": 29,
+      "pose": {
+        "translation": {
+          "x": -0.17474985122680664,
+          "y": 0.7096297740936279,
+          "z": 0.5477728247642517
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9998412728309631,
-            "X" : -0.00282590021379292,
-            "Y" : -0.006864454131573439,
-            "Z" : -0.01619541272521019
+        "rotation": {
+          "quaternion": {
+            "W": 0.9998412728309631,
+            "X": -0.00282590021379292,
+            "Y": -0.006864454131573439,
+            "Z": -0.01619541272521019
           }
         }
       }
     },
     {
-      "ID" : 30,
-      "pose" : {
-        "translation" : {
-          "x" : -0.15256865322589874,
-          "y" : 1.1327531337738037,
-          "z" : 0.548723578453064
+      "ID": 30,
+      "pose": {
+        "translation": {
+          "x": -0.15256865322589874,
+          "y": 1.1327531337738037,
+          "z": 0.548723578453064
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9996415376663208,
-            "X" : -0.002256404608488083,
-            "Y" : -0.012892674654722214,
-            "Z" : -0.0233586598187685
+        "rotation": {
+          "quaternion": {
+            "W": 0.9996415376663208,
+            "X": -0.002256404608488083,
+            "Y": -0.012892674654722214,
+            "Z": -0.0233586598187685
           }
         }
       }
     },
     {
-      "ID" : 31,
-      "pose" : {
-        "translation" : {
-          "x" : -0.006732862442731857,
-          "y" : 3.744476795196533,
-          "z" : 0.5510860085487366
+      "ID": 31,
+      "pose": {
+        "translation": {
+          "x": -0.006732862442731857,
+          "y": 3.744476795196533,
+          "z": 0.5510860085487366
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 0.9999865889549255,
-            "X" : -0.001071035978384316,
-            "Y" : -0.0008199909934774041,
-            "Z" : -0.004985099658370018
+        "rotation": {
+          "quaternion": {
+            "W": 0.9999865889549255,
+            "X": -0.001071035978384316,
+            "Y": -0.0008199909934774041,
+            "Z": -0.004985099658370018
           }
         }
       }
     },
     {
-      "ID" : 32,
-      "pose" : {
-        "translation" : {
-          "x" : 0.008077199570834637,
-          "y" : 4.1775126457214355,
-          "z" : 0.5524500012397766
+      "ID": 32,
+      "pose": {
+        "translation": {
+          "x": 0.008077199570834637,
+          "y": 4.1775126457214355,
+          "z": 0.5524500012397766
         },
-        "rotation" : {
-          "quaternion" : {
-            "W" : 1.0,
-            "X" : 0.0,
-            "Y" : 0.0,
-            "Z" : 0.0
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
           }
         }
       }
     }
   ],
-  "field" : {
-    "length" : 16.541,
-    "width" : 8.069
+  "field": {
+    "length": 16.541,
+    "width": 8.069
   },
-  "_comment" : "generated by PractiCal 1.0.5 (3) on 3/13/2026"
-}
\ No newline at end of file
+  "_comment": "generated by PractiCal 1.0.5 (3) on 3/13/2026"
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index d8857acb..0a36e448 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -903,6 +903,7 @@ public void simulationPeriodic() {
   public void disabledInit() {
     addAutos();
     System.out.println("--------------Robot Disabled-----------");
+    Superstructure.matchStartTime = 0;
   }
 
   @Override
@@ -927,7 +928,7 @@ public void autonomousExit() {
   @Override
   public void teleopInit() {
     Superstructure.matchStartTime = Timer.getFPGATimestamp();
-  
+
     intake.slapdownInit();
   }
 
diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 6c1e7174..09523be4 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -830,10 +830,10 @@ public boolean inScoringArea() {
   }
 
   public boolean canScore() {
-    return 
+    return
     // (isOurShift() || !DriverStation.isFMSAttached())
-    //     && 
-        (inScoringArea() || poseOverride)
+    //     &&
+    (inScoringArea() || poseOverride)
         && (!swerve.isNearTrench() || poseOverride || fixedShotTarget != FixedShotTarget.NONE);
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
index e479e2d8..72ad01a1 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
@@ -38,7 +38,7 @@
 public class TurretSubsystem extends SubsystemBase implements Shooter {
 
   /** Creates a new TurretSubsystem. */
-  public static final double HOOD_GEAR_RATIO = 33.8671875;//58.96875;
+  public static final double HOOD_GEAR_RATIO = 33.8671875; // 58.96875;
 
   public static final double FLYWHEEL_GEAR_RATIO = 0.84615384615;
 
diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java
index 5ca16bde..c6c9cd63 100644
--- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java
+++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java
@@ -1,7 +1,5 @@
 package frc.robot.subsystems.swerve.constants;
 
-import java.io.File;
-
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -17,6 +15,7 @@
 import edu.wpi.first.wpilibj.Filesystem;
 import frc.robot.components.camera.Camera.CameraConstants;
 import frc.robot.subsystems.swerve.module.Module.ModuleConstants;
+import java.io.File;
 
 public abstract class SwerveConstants {
 
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index eac52efd..e5849d46 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.shooter.TurretSubsystem;
 import frc.robot.utils.FieldUtils;
-import frc.robot.utils.LoggedTunableNumber;
 import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData;
 import org.littletonrobotics.junction.Logger;
 
@@ -19,8 +18,9 @@ public class AutoAim {
   private static boolean outOfRange = false; // TODO not sure if this should be true by default
 
   public static double LATENCY_COMPENSATION_SECS =
-    //   new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency comp
-    0.3;
+      //   new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency
+      // comp
+      0.3;
   //   public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time
 
   public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree();
@@ -66,44 +66,65 @@ public class AutoAim {
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12),
-        new ShotData(Rotation2d.fromDegrees(25), 35 
-        // - 6
-        , 1.14));
+        new ShotData(
+            Rotation2d.fromDegrees(25),
+            35
+            // - 6
+            ,
+            1.14));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12),
-        new ShotData(Rotation2d.fromDegrees(26), 37 
-        // - 6
-        , 1.10));
+        new ShotData(
+            Rotation2d.fromDegrees(26),
+            37
+            // - 6
+            ,
+            1.10));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12),
-        new ShotData(Rotation2d.fromDegrees(30), 37 
-        // - 6
-        , 1.09));
+        new ShotData(
+            Rotation2d.fromDegrees(30),
+            37
+            // - 6
+            ,
+            1.09));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12),
-        new ShotData(Rotation2d.fromDegrees(33), 37 
-        // - 6
-        , 1.15));
+        new ShotData(
+            Rotation2d.fromDegrees(33),
+            37
+            // - 6
+            ,
+            1.15));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12),
-        new ShotData(Rotation2d.fromDegrees(36), 38 
-        // - 6
-        , 1.23));
+        new ShotData(
+            Rotation2d.fromDegrees(36),
+            38
+            // - 6
+            ,
+            1.23));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12),
-        new ShotData(Rotation2d.fromDegrees(38), 39 
-        // - 6
-        , 1.33));
+        new ShotData(
+            Rotation2d.fromDegrees(38),
+            39
+            // - 6
+            ,
+            1.33));
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12),
-        new ShotData(Rotation2d.fromDegrees(39), 40.5 
-        // - 6
-        , 1.35));
+        new ShotData(
+            Rotation2d.fromDegrees(39),
+            40.5
+            // - 6
+            ,
+            1.35));
   }
 
   // Ig we'll see if we need more than 1 feed shot tree
@@ -111,34 +132,34 @@ public class AutoAim {
 
   static { // For feed shot tree
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 , 0)); // - 2, 0));
+        Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20, 0)); // - 2, 0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 , 0.0)); //- 2, 0.0));
+        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30, 0.0)); //- 2, 0.0));
+        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32, 0.0)); //- 2, 0.0));
+        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
         Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
         Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38, 0.0));// - 2, 0.0));
+        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40, 0.0));// - 2, 0.0));
+        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40, 0.0)); // - 2, 0.0));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38, 1.42)); //- 2, 1.42));
+        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38, 1.42)); // - 2, 1.42));
     FEED_SHOT_TREE.put(
         Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40, 1.36)); // - 2, 1.36));
     FEED_SHOT_TREE.put(
         Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41, 1.34)); // - 2, 1.34));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 , 1.25)); //- 2, 1.25));
+        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42, 1.25)); // - 2, 1.25));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43, 1.28));// - 2, 1.28));
+        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43, 1.28)); // - 2, 1.28));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45, 1.27));// - 2, 1.27));
+        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45, 1.27)); // - 2, 1.27));
     FEED_SHOT_TREE.put(
         Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46, 1.32)); // - 2, 1.32));
     FEED_SHOT_TREE.put(
@@ -147,11 +168,11 @@ public class AutoAim {
     FEED_SHOT_TREE.put(
         Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53, 1.33));//- 2, 1.33));
+        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53, 1.33)); // - 2, 1.33));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57, 1.3));// - 2, 1.3));
+        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57, 1.2));// - 2, 1.2));
+        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57, 1.2)); // - 2, 1.2));
     FEED_SHOT_TREE.put(
         Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59, 1.2)); // - 2, 1.2));
 

From a5af3c6a45d47b353ec9f005836c965433b90eb2 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:57:07 -0700
Subject: [PATCH 20/54] Update at 'Sat Mar 14 08:57:06 PDT 2026'

---
 src/main/java/frc/robot/Superstructure.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 09523be4..91be1205 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -811,6 +811,7 @@ private double getTimeLeftInShift() {
     return timeLeftinMatch() - offset;
   }
 
+  @AutoLogOutput(key = "Is our shift?")
   public boolean isOurShift() {
     // only cant score when its the others turn, otherwise everyone can
     if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) {

From fff671d00c9132db7fc77712ced8171c5c4c95f2 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:58:23 -0700
Subject: [PATCH 21/54] Update at 'Sat Mar 14 08:58:23 PDT 2026'

---
 src/main/java/frc/robot/Superstructure.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 91be1205..907127c0 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -783,6 +783,7 @@ private Alliance getStartingAlliance() {
   }
 
   private String getCurrentShift() {
+    if (DriverStation.isDisabled()) return "Disabled";
     if (130.00 < timeLeftinMatch() && timeLeftinMatch() <= 140.00) {
       return "Transition";
     } else if (105.00 < timeLeftinMatch() && timeLeftinMatch() <= 130.00) {

From 810707331591a1c0adb3402f5b338013afa84803 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:58:42 -0700
Subject: [PATCH 22/54] Update at 'Sat Mar 14 08:58:42 PDT 2026'

---
 src/main/java/frc/robot/Superstructure.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 907127c0..45ab444a 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -800,6 +800,7 @@ private String getCurrentShift() {
   }
 
   private double getTimeLeftInShift() {
+    if (DriverStation.isDisabled()) return 0;
     double offset =
         switch (getCurrentShift()) {
           case "Transition" -> 130.00;

From a55b5aeca370698c68e5c117b863849eca83ca1f Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 08:59:36 -0700
Subject: [PATCH 23/54] Update at 'Sat Mar 14 08:59:36 PDT 2026'

---
 src/main/java/frc/robot/Superstructure.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 45ab444a..6dd37eb9 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -815,6 +815,7 @@ private double getTimeLeftInShift() {
 
   @AutoLogOutput(key = "Is our shift?")
   public boolean isOurShift() {
+    if (DriverStation.isDisabled()) return false;
     // only cant score when its the others turn, otherwise everyone can
     if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) {
       return !(getCurrentShift() == "Shift 2" || getCurrentShift() == "Shift 4");

From e7d65d6e5d09a91f39a15f567c78e0ac3a53bcad Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 10:19:50 -0700
Subject: [PATCH 24/54] add back -6 to flywheel velocity

---
 src/main/java/frc/robot/utils/autoaim/AutoAim.java | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index e5849d46..33472655 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -69,7 +69,7 @@ public class AutoAim {
         new ShotData(
             Rotation2d.fromDegrees(25),
             35
-            // - 6
+             - 6
             ,
             1.14));
 
@@ -77,7 +77,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12),
         new ShotData(
             Rotation2d.fromDegrees(26),
-            37
+            37 - 6
             // - 6
             ,
             1.10));
@@ -86,7 +86,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12),
         new ShotData(
             Rotation2d.fromDegrees(30),
-            37
+            37 -6
             // - 6
             ,
             1.09));
@@ -95,7 +95,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12),
         new ShotData(
             Rotation2d.fromDegrees(33),
-            37
+            37 -6
             // - 6
             ,
             1.15));
@@ -104,7 +104,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12),
         new ShotData(
             Rotation2d.fromDegrees(36),
-            38
+            38 -6
             // - 6
             ,
             1.23));
@@ -113,7 +113,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12),
         new ShotData(
             Rotation2d.fromDegrees(38),
-            39
+            39 -6
             // - 6
             ,
             1.33));
@@ -121,7 +121,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12),
         new ShotData(
             Rotation2d.fromDegrees(39),
-            40.5
+            40.5 -6 
             // - 6
             ,
             1.35));

From c423a90ec76e2f2895a6025836f3fa7f69d15c8c Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 11:09:45 -0700
Subject: [PATCH 25/54] add no log stick alert and also decrease shot vel a bit

---
 src/main/java/frc/robot/Robot.java               | 11 ++++++++++-
 .../java/frc/robot/utils/autoaim/AutoAim.java    | 16 ++++++++--------
 2 files changed, 18 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0a36e448..d2beb775 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -8,6 +8,8 @@
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
 import com.playingwithfusion.BattFuelGauge;
+
+import edu.wpi.first.hal.simulation.RoboRioDataJNI;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -71,6 +73,8 @@
 import frc.robot.utils.FieldUtils.FeedTargets;
 import frc.robot.utils.FieldUtils.TrenchPoses;
 import frc.robot.utils.autoaim.AutoAim;
+
+import java.io.File;
 import java.util.Arrays;
 import java.util.Optional;
 import java.util.Set;
@@ -79,6 +83,7 @@
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
@@ -108,7 +113,9 @@ public enum RobotEdition {
   public static final RobotEdition REPLAY_ROBOT_EDITION = RobotEdition.ALPHA;
   private static final Alert unknownRioAlert =
       new Alert("!! Unknown Rio detected. Defaulting to comp", AlertType.kError);
+      private static final Alert noLogStickAlert = new Alert("NO LOG STICK!! POWER OFF BEFORE PLUGGING IT IN", AlertType.kError);
 
+      File directory = new File("/U");
   // for replay to work properly this needs to match the edition in the log
   static {
     switch (ROBOT_MODE) {
@@ -455,7 +462,7 @@ public Robot() {
     // set up logging stuff depending on robot mode
     switch (ROBOT_MODE) {
       case REAL:
-        Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick
+        Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick)
         Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
         // TODO confirm pdp vs pdh
         // apparently LoggedPowerDistribution doesn't work with the pdp 2.0
@@ -814,6 +821,8 @@ public void robotPeriodic() {
         Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new));
 
     Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone());
+
+    noLogStickAlert.set(!directory.exists());
   }
 
   public void updateAlerts() {
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 33472655..b66a699b 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -62,14 +62,14 @@ public class AutoAim {
   static { // For hub shot tree
     // TODO min shot
     COMP_HUB_SHOT_TREE.put(
-        Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6, 1.04));
+        Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6 + 3, 1.04));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12),
         new ShotData(
             Rotation2d.fromDegrees(25),
             35
-             - 6
+             - 6 + 3
             ,
             1.14));
 
@@ -77,7 +77,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12),
         new ShotData(
             Rotation2d.fromDegrees(26),
-            37 - 6
+            37 - 6 + 3
             // - 6
             ,
             1.10));
@@ -86,7 +86,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12),
         new ShotData(
             Rotation2d.fromDegrees(30),
-            37 -6
+            37 -6 + 3
             // - 6
             ,
             1.09));
@@ -95,7 +95,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12),
         new ShotData(
             Rotation2d.fromDegrees(33),
-            37 -6
+            37 -6 + 3
             // - 6
             ,
             1.15));
@@ -104,7 +104,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12),
         new ShotData(
             Rotation2d.fromDegrees(36),
-            38 -6
+            38 -6 + 3
             // - 6
             ,
             1.23));
@@ -113,7 +113,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12),
         new ShotData(
             Rotation2d.fromDegrees(38),
-            39 -6
+            39 -6 + 3
             // - 6
             ,
             1.33));
@@ -121,7 +121,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12),
         new ShotData(
             Rotation2d.fromDegrees(39),
-            40.5 -6 
+            40.5 -6  + 3
             // - 6
             ,
             1.35));

From d14f2ca4dd7648b9d049fb0e54eb3d4320124676 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 11:11:04 -0700
Subject: [PATCH 26/54] Update at 'Sat Mar 14 11:11:03 PDT 2026'

---
 src/main/java/frc/robot/Robot.java            | 10 ++++------
 .../java/frc/robot/utils/autoaim/AutoAim.java | 20 ++++++++-----------
 2 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index d2beb775..8aea56ea 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -8,8 +8,6 @@
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
 import com.playingwithfusion.BattFuelGauge;
-
-import edu.wpi.first.hal.simulation.RoboRioDataJNI;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -73,7 +71,6 @@
 import frc.robot.utils.FieldUtils.FeedTargets;
 import frc.robot.utils.FieldUtils.TrenchPoses;
 import frc.robot.utils.autoaim.AutoAim;
-
 import java.io.File;
 import java.util.Arrays;
 import java.util.Optional;
@@ -83,7 +80,6 @@
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
@@ -113,9 +109,11 @@ public enum RobotEdition {
   public static final RobotEdition REPLAY_ROBOT_EDITION = RobotEdition.ALPHA;
   private static final Alert unknownRioAlert =
       new Alert("!! Unknown Rio detected. Defaulting to comp", AlertType.kError);
-      private static final Alert noLogStickAlert = new Alert("NO LOG STICK!! POWER OFF BEFORE PLUGGING IT IN", AlertType.kError);
+  private static final Alert noLogStickAlert =
+      new Alert("NO LOG STICK!! POWER OFF BEFORE PLUGGING IT IN", AlertType.kError);
+
+  File directory = new File("/U");
 
-      File directory = new File("/U");
   // for replay to work properly this needs to match the edition in the log
   static {
     switch (ROBOT_MODE) {
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index b66a699b..e25a777a 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -62,16 +62,12 @@ public class AutoAim {
   static { // For hub shot tree
     // TODO min shot
     COMP_HUB_SHOT_TREE.put(
-        Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6 + 3, 1.04));
+        Units.inchesToMeters(24 + 17),
+        new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6 + 3, 1.04));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12),
-        new ShotData(
-            Rotation2d.fromDegrees(25),
-            35
-             - 6 + 3
-            ,
-            1.14));
+        new ShotData(Rotation2d.fromDegrees(25), 35 - 6 + 3, 1.14));
 
     COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12),
@@ -86,7 +82,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12),
         new ShotData(
             Rotation2d.fromDegrees(30),
-            37 -6 + 3
+            37 - 6 + 3
             // - 6
             ,
             1.09));
@@ -95,7 +91,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12),
         new ShotData(
             Rotation2d.fromDegrees(33),
-            37 -6 + 3
+            37 - 6 + 3
             // - 6
             ,
             1.15));
@@ -104,7 +100,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12),
         new ShotData(
             Rotation2d.fromDegrees(36),
-            38 -6 + 3
+            38 - 6 + 3
             // - 6
             ,
             1.23));
@@ -113,7 +109,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12),
         new ShotData(
             Rotation2d.fromDegrees(38),
-            39 -6 + 3
+            39 - 6 + 3
             // - 6
             ,
             1.33));
@@ -121,7 +117,7 @@ public class AutoAim {
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12),
         new ShotData(
             Rotation2d.fromDegrees(39),
-            40.5 -6  + 3
+            40.5 - 6 + 3
             // - 6
             ,
             1.35));

From 5cbaa5cbd75a55074a9b003f7af6f84524b57c29 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 12:06:11 -0700
Subject: [PATCH 27/54] i'm really really stupid

---
 src/main/java/frc/robot/Autos.java | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 0d976eee..88631ace 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -220,7 +220,7 @@ public Command climbPath(Path path, AutoRoutine routine) {
         setAutoPreClimbReqTrue(),
         // Commands.parallel(
         path.getTrajectory(routine)
-            .cmd()
+            .cmd() 
             .until(
                 // routine.observe(
                 //     path.getTrajectory(routine)
@@ -228,13 +228,14 @@ public Command climbPath(Path path, AutoRoutine routine) {
                 //             path.getTrajectory(routine).getRawTrajectory().getTotalTime()
                 //                 - (0.3)))),
                 path.getTrajectory(routine).done()),
-        Commands.parallel(swerve.stop(), setAutoScoreReqTrue()).repeatedly().withTimeout(4),
+        Commands.parallel(swerve.stop(), setAutoScoreReqTrue()).repeatedly().withTimeout(2.5),
+        setAutoScoreReqFalse(),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
             Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
                 .andThen(
-                    Commands.print("hooray!")
-                    // setAutoClimbReqTrue()
+                    // Commands.print("hooray!")
+                    setAutoClimbReqTrue()
                     )));
   }
 

From 189d3575076914dac4271e07b3a049431aea8f21 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:18:23 -0700
Subject: [PATCH 28/54] Update at 'Sat Mar 14 13:18:23 PDT 2026'

---
 src/main/java/frc/robot/Autos.java                           | 5 ++---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java  | 4 ++--
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index d295e5c9..792144dd 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -222,7 +222,7 @@ public Command climbPath(Path path, AutoRoutine routine) {
         setAutoPreClimbReqTrue(),
         // Commands.parallel(
         path.getTrajectory(routine)
-            .cmd() 
+            .cmd()
             .until(
                 // routine.observe(
                 //     path.getTrajectory(routine)
@@ -237,8 +237,7 @@ public Command climbPath(Path path, AutoRoutine routine) {
             Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
                 .andThen(
                     // Commands.print("hooray!")
-                    setAutoClimbReqTrue()
-                    )));
+                    setAutoClimbReqTrue())));
   }
 
   public Command feedPath(Path path, AutoRoutine routine) {
diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index eaf9622f..112b5869 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -94,14 +94,14 @@ public Command agitate() {
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
-                      rollerIO.setRollerVelocity(10.0);
+                      rollerIO.setRollerVelocity(80.0);
                     })
                 .until(atExtensionTrigger),
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(
                           PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(40)));
-                      rollerIO.setRollerVelocity(10.0);
+                      rollerIO.setRollerVelocity(80.0);
                     })
                 .until(atExtensionTrigger))
         .repeatedly();

From 378fea6aa3f6e30c35663b67ac01ad181db9ee76 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:26:21 -0700
Subject: [PATCH 29/54] Update at 'Sat Mar 14 13:26:21 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index 112b5869..3ac49c8e 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -94,14 +94,14 @@ public Command agitate() {
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
-                      rollerIO.setRollerVelocity(80.0);
+                      rollerIO.setRollerVelocity(50.0);
                     })
                 .until(atExtensionTrigger),
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(
                           PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(40)));
-                      rollerIO.setRollerVelocity(80.0);
+                      rollerIO.setRollerVelocity(50.0);
                     })
                 .until(atExtensionTrigger))
         .repeatedly();

From 1cb64f717138f335f593c4109758f583f3d117cd Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:40:39 -0700
Subject: [PATCH 30/54] Update at 'Sat Mar 14 13:40:39 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index 3ac49c8e..f95ccd35 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -210,8 +210,8 @@ public static TalonFXConfiguration getPivotConfig() {
     config.CurrentLimits.SupplyCurrentLimitEnable = true;
 
     // TODO: TUNE
-    config.MotionMagic.MotionMagicCruiseVelocity = 5;
-    config.MotionMagic.MotionMagicAcceleration = 5;
+    config.MotionMagic.MotionMagicCruiseVelocity = 10;
+    config.MotionMagic.MotionMagicAcceleration = 10;
 
     return config;
   }

From a3db675cb0efc56e84fe457739842d0795d3411a Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:42:52 -0700
Subject: [PATCH 31/54] Update at 'Sat Mar 14 13:42:52 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index f95ccd35..6766fb64 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -100,7 +100,7 @@ public Command agitate() {
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(
-                          PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(40)));
+                          PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(20)));
                       rollerIO.setRollerVelocity(50.0);
                     })
                 .until(atExtensionTrigger))

From 1924eda0baf737be5241ed11053235103b2de3b8 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:45:55 -0700
Subject: [PATCH 32/54] Update at 'Sat Mar 14 13:45:55 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index 6766fb64..c57faec0 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -94,14 +94,14 @@ public Command agitate() {
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
-                      rollerIO.setRollerVelocity(50.0);
+                      // rollerIO.setRollerVelocity(50.0);
                     })
                 .until(atExtensionTrigger),
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(
                           PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(20)));
-                      rollerIO.setRollerVelocity(50.0);
+                      // rollerIO.setRollerVelocity(50.0);
                     })
                 .until(atExtensionTrigger))
         .repeatedly();

From 71f5b77fed444b89d137a30c18ff1c1ef5fa33a7 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:48:09 -0700
Subject: [PATCH 33/54] Update at 'Sat Mar 14 13:48:08 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index c57faec0..c92a76cd 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -94,14 +94,14 @@ public Command agitate() {
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
-                      // rollerIO.setRollerVelocity(50.0);
+                      rollerIO.setRollerVelocity(10.0);
                     })
                 .until(atExtensionTrigger),
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(
-                          PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(20)));
-                      // rollerIO.setRollerVelocity(50.0);
+                          PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(30)));
+                      rollerIO.setRollerVelocity(10.0);
                     })
                 .until(atExtensionTrigger))
         .repeatedly();

From 128ee88d1aaee6b1351a6b02c1de0b856e8bb44a Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 13:49:34 -0700
Subject: [PATCH 34/54] Update at 'Sat Mar 14 13:49:34 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index c92a76cd..2f9304c2 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -100,7 +100,7 @@ public Command agitate() {
             this.run(
                     () -> {
                       pivotIO.setMotorPositionSetpoint(
-                          PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(30)));
+                          PIVOT_EXTENDED_POSITION.plus(Rotation2d.fromDegrees(40)));
                       rollerIO.setRollerVelocity(10.0);
                     })
                 .until(atExtensionTrigger))

From e20b2ec269d749adbdd6a263b0a82fe1638d5353 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 14:01:59 -0700
Subject: [PATCH 35/54] Update at 'Sat Mar 14 14:01:58 PDT 2026'

---
 src/main/deploy/choreo/BtoD.traj   | 107 +++++++++++++++++++++++++++++
 src/main/java/frc/robot/Autos.java |  20 +++++-
 src/main/java/frc/robot/Robot.java |   1 +
 3 files changed, 127 insertions(+), 1 deletion(-)
 create mode 100644 src/main/deploy/choreo/BtoD.traj

diff --git a/src/main/deploy/choreo/BtoD.traj b/src/main/deploy/choreo/BtoD.traj
new file mode 100644
index 00000000..a26252cc
--- /dev/null
+++ b/src/main/deploy/choreo/BtoD.traj
@@ -0,0 +1,107 @@
+{
+ "name":"BtoD",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.631879568099976, "y":5.975247383117676, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.7998199462890625, "y":5.975247383117676, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.703999767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
+    {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"3.6318795680999756 m", "val":3.631879568099976}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"D.x", "val":0.703999767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.0}}}, "enabled":true},
+    {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slower", "val":1.5}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2761996,
+    "y":0.2761996
+   },
+   "backLeft":{
+    "x":-0.2761996,
+    "y":0.2761996
+   },
+   "mass":66.133767546,
+   "inertia":10.42384825260472,
+   "gearing":7.03,
+   "radius":0.0508,
+   "vmax":485.8996637552213,
+   "tmax":0.9,
+   "cof":2.25,
+   "bumper":{
+    "front":0.44196,
+    "side":0.44196,
+    "back":0.44196
+   },
+   "differentialTrackWidth":0.5427218
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.05853,1.89013],
+  "samples":[
+    {"t":0.0, "x":3.63188, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52604, "ay":0.0, "alpha":0.0, "fx":[-124.43129,-124.43129,-124.43129,-124.43129], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.04411, "x":3.62456, "y":5.97525, "heading":3.14159, "vx":-0.33194, "vy":0.0, "omega":0.0, "ax":-7.52462, "ay":0.0, "alpha":0.0, "fx":[-124.40791,-124.40791,-124.40791,-124.40791], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.08821, "x":3.6026, "y":5.97525, "heading":3.14159, "vx":-0.66381, "vy":0.0, "omega":0.0, "ax":-7.52249, "ay":0.0, "alpha":0.0, "fx":[-124.37271,-124.37271,-124.37271,-124.37271], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.13232, "x":3.56601, "y":5.97525, "heading":3.14159, "vx":-0.99559, "vy":0.0, "omega":0.0, "ax":-7.51893, "ay":0.0, "alpha":0.0, "fx":[-124.31375,-124.31375,-124.31375,-124.31375], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.17642, "x":3.51478, "y":5.97525, "heading":3.14159, "vx":-1.32722, "vy":0.0, "omega":0.0, "ax":-7.51173, "ay":0.0, "alpha":0.0, "fx":[-124.19469,-124.19469,-124.19469,-124.19469], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.22053, "x":3.44894, "y":5.97525, "heading":3.14159, "vx":-1.65853, "vy":0.0, "omega":0.0, "ax":-7.48958, "ay":0.0, "alpha":0.0, "fx":[-123.82854,-123.82854,-123.82854,-123.82854], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.26463, "x":3.3685, "y":5.97525, "heading":3.14159, "vx":-1.98886, "vy":0.0, "omega":0.0, "ax":-0.24222, "ay":0.0, "alpha":0.0, "fx":[-4.0048,-4.0048,-4.0048,-4.0048], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.30874, "x":3.28055, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00006,-0.00006,-0.00006,-0.00006], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.35284, "x":3.19236, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39695, "x":3.10417, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.44105, "x":3.01598, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.48516, "x":2.92779, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.52926, "x":2.8396, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.57337, "x":2.75141, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.61747, "x":2.66322, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.66158, "x":2.57503, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.70568, "x":2.48684, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.74979, "x":2.39865, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.79389, "x":2.31046, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.838, "x":2.22227, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.88211, "x":2.13408, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.92621, "x":2.04589, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.00008, "ay":0.0, "alpha":0.0, "fx":[0.00133,0.00133,0.00133,0.00133], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.97032, "x":1.9577, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":3.844, "ay":0.0, "alpha":0.0, "fx":[63.55458,63.55458,63.55458,63.55458], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.01442, "x":1.87325, "y":5.97525, "heading":3.14159, "vx":-1.82999, "vy":0.0, "omega":0.0, "ax":7.49157, "ay":0.0, "alpha":0.0, "fx":[123.8614,123.8614,123.8614,123.8614], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.05853, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49958, "vy":0.0, "omega":0.0, "ax":-0.00187, "ay":0.0, "alpha":0.0, "fx":[-0.03093,-0.03093,-0.03093,-0.03093], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.10229, "x":1.73418, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.14606, "x":1.66855, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.18983, "x":1.60291, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.2336, "x":1.53727, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.27737, "x":1.47163, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.32114, "x":1.40599, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.36491, "x":1.34036, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.40867, "x":1.27472, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.45244, "x":1.20908, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.49621, "x":1.14344, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.53998, "x":1.0778, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.58375, "x":1.01217, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.62752, "x":0.94653, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.00005, "ay":0.0, "alpha":0.0, "fx":[0.00087,0.00087,0.00087,0.00087], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.67129, "x":0.88089, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":4.19573, "ay":0.0, "alpha":0.0, "fx":[69.36979,69.36979,69.36979,69.36979], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.71505, "x":0.81927, "y":5.97525, "heading":3.14159, "vx":-1.31602, "vy":0.0, "omega":0.0, "ax":7.50211, "ay":0.0, "alpha":0.0, "fx":[124.03571,124.03571,124.03571,124.03571], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.75882, "x":0.76886, "y":5.97525, "heading":3.14159, "vx":-0.98766, "vy":0.0, "omega":0.0, "ax":7.51752, "ay":0.0, "alpha":0.0, "fx":[124.29053,124.29053,124.29053,124.29053], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.80259, "x":0.73283, "y":5.97525, "heading":3.14159, "vx":-0.65863, "vy":0.0, "omega":0.0, "ax":7.52269, "ay":0.0, "alpha":0.0, "fx":[124.37591,124.37591,124.37591,124.37591], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.84636, "x":0.71121, "y":5.97525, "heading":3.14159, "vx":-0.32937, "vy":0.0, "omega":0.0, "ax":7.52527, "ay":0.0, "alpha":0.0, "fx":[124.41868,124.41868,124.41868,124.41868], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.89013, "x":0.704, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 792144dd..09cdf3ee 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -136,7 +136,9 @@ public enum Path {
     OtoCR("O", "CR", Action.CLIMB),
     DtoCL("D", "CL", Action.CLIMB),
 
-    RUNtoTEST("RUN", "TEST", Action.NOTHING);
+    RUNtoTEST("RUN", "TEST", Action.NOTHING),
+
+    BtoD("B", "D", Action.INTAKE);
 
     private final String start;
     private final String end;
@@ -548,6 +550,22 @@ public Command getDepotOutpostClimbAuto() {
     return routine.cmd();
   }
 
+  public Command getBumpOutpostClimbAuto() {
+    final AutoRoutine routine = factory.newRoutine("Bump Outpost Climb Auto");
+    lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
+    Path[] paths = {Path.BtoD, Path.DtoS, Path.StoO, Path.OtoCR};
+    Command autoCommand =
+        paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse());
+
+    for (Path p : paths) {
+      autoCommand = autoCommand.andThen(runPath(p, routine));
+    }
+
+    routine.active().whileTrue(autoCommand);
+
+    return routine.cmd();
+  }
+
   public Command getCenterScoreAuto() {
     final AutoRoutine routine = factory.newRoutine("Center Score Auto");
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a276daf4..06f74f50 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -735,6 +735,7 @@ private void addAutos() {
     autoChooser.addOption("Score in Center", autos.getCenterScoreAuto());
     autoChooser.addOption("Test Auto", autos.getTestAuto());
     autoChooser.addOption("Just Score", autos.getJustScoreAuto());
+    autoChooser.addOption("Bump Outpost Climb", autos.getBumpOutpostClimbAuto());
 
     haveAutosGenerated = true;
     System.out.println("Done generating autos");

From d4347867e8114e70b7a9b84bdf8c5e9ff0c94daa Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 14:02:42 -0700
Subject: [PATCH 36/54] Update at 'Sat Mar 14 14:02:42 PDT 2026'

---
 src/main/java/frc/robot/Robot.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 06f74f50..0b7bce41 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -735,7 +735,7 @@ private void addAutos() {
     autoChooser.addOption("Score in Center", autos.getCenterScoreAuto());
     autoChooser.addOption("Test Auto", autos.getTestAuto());
     autoChooser.addOption("Just Score", autos.getJustScoreAuto());
-    autoChooser.addOption("Bump Outpost Climb", autos.getBumpOutpostClimbAuto());
+    autoChooser.addOption("Bump Depot Outpost Climb", autos.getBumpOutpostClimbAuto());
 
     haveAutosGenerated = true;
     System.out.println("Done generating autos");

From 8c1776250c4e90be0eed224acfdedb2f179b7ce4 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 14:45:03 -0700
Subject: [PATCH 37/54] add rumble to op

---
 src/main/java/frc/robot/Robot.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0b7bce41..b3b877ac 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -651,7 +651,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                         shooter.runHoodCurrentZeroing(), intake.runCurrentZeroing())));
 
     new Trigger(() -> AutoAim.targetInTurretDeadzone())
-        .onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25));
+        .onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25).alongWith(operator.rumbleCmd(1, 1).withTimeout(0.25)));
     // ---zeroing stuff---
     driver.povUp().whileTrue(shooter.currentZeroTurretAgainstForwardHardstop());
 

From f9916b34c73742ec9fc8d427a960e39eeee4efc9 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 14:50:06 -0700
Subject: [PATCH 38/54] swerve align for feed also

---
 src/main/java/frc/robot/Robot.java            | 22 ++++++++++++-
 src/main/java/frc/robot/Superstructure.java   |  7 ++++
 .../subsystems/swerve/SwerveSubsystem.java    | 33 +++++++++++++++++++
 3 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index b3b877ac..281af9c9 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -694,6 +694,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
     new Trigger(AutoAim::targetInTurretDeadzone)
         .and(() -> Superstructure.getState().isAScoreState())
         .and(() -> !Superstructure.getPoseOverride())
+        .and(() -> superstructure.inScoringArea())
         .whileTrue(
             swerve.faceHubComp(
                 () ->
@@ -704,7 +705,26 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                     -1
                         * modifyJoystick(driver.getLeftX())
                         * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
-                shooter::getTurretPosition));
+                shooter::getTurretPosition));  
+
+    new Trigger(AutoAim::targetInTurretDeadzone)
+        .and(() -> Superstructure.getState().isAFeedState())
+        .and(() -> !Superstructure.getPoseOverride())
+        .and(() -> !superstructure.inScoringArea())
+        .whileTrue(
+            swerve.faceFeedComp(
+                () ->
+                    -1
+                        * modifyJoystick(driver.getLeftY())
+                        * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
+                () ->
+                    -1
+                        * modifyJoystick(driver.getLeftX())
+                        * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
+                shooter::getTurretPosition,
+                 () -> Superstructure.getFeedTarget()));  
+                
+                
 
     // create triggers for joystick disconnect alerts
     new Trigger(() -> DriverStation.isJoystickConnected(0))
diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 6dd37eb9..0ca290c2 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -63,6 +63,13 @@ public boolean isAScoreState() {
           || this == SPIN_UP_SCORE_FLOW
           || this == SCORE_FLOW;
     }
+
+    public boolean isAFeedState() {
+      return this == FEED
+          || this == SPIN_UP_FEED
+          || this == SPIN_UP_FEED_FLOW
+          || this == FEED_FLOW;
+    }
   }
 
   public enum ShotTarget {
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 5e7a1fcf..287f344a 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -38,6 +38,7 @@
 import frc.robot.Robot;
 import frc.robot.Robot.RobotEdition;
 import frc.robot.Robot.RobotMode;
+import frc.robot.Superstructure.FeedTarget;
 import frc.robot.Superstructure;
 import frc.robot.components.camera.Camera;
 import frc.robot.components.camera.CameraIOReal;
@@ -735,6 +736,38 @@ public Command faceHubComp(
         yVel);
   }
 
+  public Command faceFeedComp(
+      DoubleSupplier xVel, DoubleSupplier yVel, Supplier<Rotation2d> turretRotation, Supplier<FeedTarget> feedTargetSupplier) {
+    return driveWithHeadingSnap(
+        () -> {
+          Pose2d turretPose =
+              getPose()
+                  .transformBy(
+                      new Transform2d(
+                          TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero));
+
+          // get desired rotation to point at target
+          Rotation2d turretTargetRotation =
+              AutoAim.getVirtualTargetYaw(
+                  getVelocityFieldRelative(),
+                  FeedTargets.getFeedTarget(feedTargetSupplier.get()).getTranslation(),
+                  turretPose,
+                  AutoAim.FEED_SHOT_TREE);
+          // subtract that from rotation to point at target
+          turretTargetRotation = turretTargetRotation.minus(getRotation());
+          Logger.recordOutput("Turret/Unclamped target", turretTargetRotation);
+          Rotation2d diff = turretTargetRotation.minus(turretRotation.get());
+          Logger.recordOutput("Turret/diff", diff);
+          // if (diff.getDegrees() > 0) {
+          //   diff = Rotation2d.fromDegrees(-diff.getDegrees());
+          // }
+          return diff.plus(getRotation());
+        },
+        xVel,
+        yVel);
+  }
+
+
   public boolean isFacingTarget(InterpolatingShotTree tree) {
     switch (Superstructure.getShotTarget()) { // ugh maybe this should be in robot.java
       case SCORE:

From 586771629b4345b61fda8d5614a2a5155c99e9a1 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 14:52:01 -0700
Subject: [PATCH 39/54] adjust outpost and depot poses

---
 src/main/deploy/choreo/BtoD.traj    |  57 ++++++-------
 src/main/deploy/choreo/NewPath.traj |  28 -------
 src/main/deploy/choreo/StoO.traj    | 121 ++++++++++++++--------------
 3 files changed, 91 insertions(+), 115 deletions(-)
 delete mode 100644 src/main/deploy/choreo/NewPath.traj

diff --git a/src/main/deploy/choreo/BtoD.traj b/src/main/deploy/choreo/BtoD.traj
index a26252cc..5ba90945 100644
--- a/src/main/deploy/choreo/BtoD.traj
+++ b/src/main/deploy/choreo/BtoD.traj
@@ -4,8 +4,8 @@
  "snapshot":{
   "waypoints":[
     {"x":3.631879568099976, "y":5.975247383117676, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.7998199462890625, "y":5.975247383117676, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":0.703999767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":1.7998199462890625, "y":5.975247383117676, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.602399767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -17,8 +17,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"3.6318795680999756 m", "val":3.631879568099976}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"D.x", "val":0.703999767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"D.x - 4 in", "val":0.602399767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -55,7 +55,7 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.05853,1.89013],
+  "waypoints":[0.0,1.05853,1.95793],
   "samples":[
     {"t":0.0, "x":3.63188, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52604, "ay":0.0, "alpha":0.0, "fx":[-124.43129,-124.43129,-124.43129,-124.43129], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.04411, "x":3.62456, "y":5.97525, "heading":3.14159, "vx":-0.33194, "vy":0.0, "omega":0.0, "ax":-7.52462, "ay":0.0, "alpha":0.0, "fx":[-124.40791,-124.40791,-124.40791,-124.40791], "fy":[0.0,0.0,0.0,0.0]},
@@ -63,7 +63,7 @@
     {"t":0.13232, "x":3.56601, "y":5.97525, "heading":3.14159, "vx":-0.99559, "vy":0.0, "omega":0.0, "ax":-7.51893, "ay":0.0, "alpha":0.0, "fx":[-124.31375,-124.31375,-124.31375,-124.31375], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.17642, "x":3.51478, "y":5.97525, "heading":3.14159, "vx":-1.32722, "vy":0.0, "omega":0.0, "ax":-7.51173, "ay":0.0, "alpha":0.0, "fx":[-124.19469,-124.19469,-124.19469,-124.19469], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.22053, "x":3.44894, "y":5.97525, "heading":3.14159, "vx":-1.65853, "vy":0.0, "omega":0.0, "ax":-7.48958, "ay":0.0, "alpha":0.0, "fx":[-123.82854,-123.82854,-123.82854,-123.82854], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.26463, "x":3.3685, "y":5.97525, "heading":3.14159, "vx":-1.98886, "vy":0.0, "omega":0.0, "ax":-0.24222, "ay":0.0, "alpha":0.0, "fx":[-4.0048,-4.0048,-4.0048,-4.0048], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.26463, "x":3.3685, "y":5.97525, "heading":3.14159, "vx":-1.98886, "vy":0.0, "omega":0.0, "ax":-0.24223, "ay":0.0, "alpha":0.0, "fx":[-4.00486,-4.00486,-4.00486,-4.00486], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.30874, "x":3.28055, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00006,-0.00006,-0.00006,-0.00006], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.35284, "x":3.19236, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.39695, "x":3.10417, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
@@ -79,28 +79,29 @@
     {"t":0.838, "x":2.22227, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.88211, "x":2.13408, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.92621, "x":2.04589, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.00008, "ay":0.0, "alpha":0.0, "fx":[0.00133,0.00133,0.00133,0.00133], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.97032, "x":1.9577, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":3.844, "ay":0.0, "alpha":0.0, "fx":[63.55458,63.55458,63.55458,63.55458], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.01442, "x":1.87325, "y":5.97525, "heading":3.14159, "vx":-1.82999, "vy":0.0, "omega":0.0, "ax":7.49157, "ay":0.0, "alpha":0.0, "fx":[123.8614,123.8614,123.8614,123.8614], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.05853, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49958, "vy":0.0, "omega":0.0, "ax":-0.00187, "ay":0.0, "alpha":0.0, "fx":[-0.03093,-0.03093,-0.03093,-0.03093], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.10229, "x":1.73418, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.14606, "x":1.66855, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.18983, "x":1.60291, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.2336, "x":1.53727, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.27737, "x":1.47163, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.32114, "x":1.40599, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.36491, "x":1.34036, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.40867, "x":1.27472, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.45244, "x":1.20908, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.49621, "x":1.14344, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.53998, "x":1.0778, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.58375, "x":1.01217, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.62752, "x":0.94653, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.00005, "ay":0.0, "alpha":0.0, "fx":[0.00087,0.00087,0.00087,0.00087], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.67129, "x":0.88089, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":4.19573, "ay":0.0, "alpha":0.0, "fx":[69.36979,69.36979,69.36979,69.36979], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.71505, "x":0.81927, "y":5.97525, "heading":3.14159, "vx":-1.31602, "vy":0.0, "omega":0.0, "ax":7.50211, "ay":0.0, "alpha":0.0, "fx":[124.03571,124.03571,124.03571,124.03571], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.75882, "x":0.76886, "y":5.97525, "heading":3.14159, "vx":-0.98766, "vy":0.0, "omega":0.0, "ax":7.51752, "ay":0.0, "alpha":0.0, "fx":[124.29053,124.29053,124.29053,124.29053], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.80259, "x":0.73283, "y":5.97525, "heading":3.14159, "vx":-0.65863, "vy":0.0, "omega":0.0, "ax":7.52269, "ay":0.0, "alpha":0.0, "fx":[124.37591,124.37591,124.37591,124.37591], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.84636, "x":0.71121, "y":5.97525, "heading":3.14159, "vx":-0.32937, "vy":0.0, "omega":0.0, "ax":7.52527, "ay":0.0, "alpha":0.0, "fx":[124.41868,124.41868,124.41868,124.41868], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.89013, "x":0.704, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.97032, "x":1.9577, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":3.84394, "ay":0.0, "alpha":0.0, "fx":[63.55357,63.55357,63.55357,63.55357], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.01442, "x":1.87325, "y":5.97525, "heading":3.14159, "vx":-1.83, "vy":0.0, "omega":0.0, "ax":7.49157, "ay":0.0, "alpha":0.0, "fx":[123.8614,123.8614,123.8614,123.8614], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.05853, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49958, "vy":0.0, "omega":0.0, "ax":-0.00192, "ay":0.0, "alpha":0.0, "fx":[-0.03168,-0.03168,-0.03168,-0.03168], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.1035, "x":1.73238, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.14847, "x":1.66494, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.19344, "x":1.5975, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.23841, "x":1.53006, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.28338, "x":1.46262, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.32835, "x":1.39518, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.37332, "x":1.32774, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.41829, "x":1.2603, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.46326, "x":1.19286, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.50823, "x":1.12542, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.5532, "x":1.05798, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.59817, "x":0.99054, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.64314, "x":0.9231, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.68811, "x":0.85566, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.00003, "ay":0.0, "alpha":0.0, "fx":[0.00052,0.00052,0.00052,0.00052], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.73308, "x":0.78822, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":3.27759, "ay":0.0, "alpha":0.0, "fx":[54.18983,54.18983,54.18983,54.18983], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.77805, "x":0.7241, "y":5.97525, "heading":3.14159, "vx":-1.35227, "vy":0.0, "omega":0.0, "ax":7.50347, "ay":0.0, "alpha":0.0, "fx":[124.05823,124.05823,124.05823,124.05823], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.82302, "x":0.67087, "y":5.97525, "heading":3.14159, "vx":-1.01484, "vy":0.0, "omega":0.0, "ax":7.51823, "ay":0.0, "alpha":0.0, "fx":[124.30219,124.30219,124.30219,124.30219], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.86799, "x":0.63284, "y":5.97525, "heading":3.14159, "vx":-0.67674, "vy":0.0, "omega":0.0, "ax":7.52316, "ay":0.0, "alpha":0.0, "fx":[124.38377,124.38377,124.38377,124.38377], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.91296, "x":0.61001, "y":5.97525, "heading":3.14159, "vx":-0.33843, "vy":0.0, "omega":0.0, "ax":7.52563, "ay":0.0, "alpha":0.0, "fx":[124.42461,124.42461,124.42461,124.42461], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.95793, "x":0.6024, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj
deleted file mode 100644
index c032e9db..00000000
--- a/src/main/deploy/choreo/NewPath.traj
+++ /dev/null
@@ -1,28 +0,0 @@
-{
- "name":"NewPath",
- "version":3,
- "snapshot":{
-  "waypoints":[],
-  "constraints":[],
-  "targetDt":0.05
- },
- "params":{
-  "waypoints":[],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
-  "targetDt":{
-   "exp":"0.05 s",
-   "val":0.05
-  }
- },
- "trajectory":{
-  "config":null,
-  "sampleType":null,
-  "waypoints":[],
-  "samples":[],
-  "splits":[]
- },
- "events":[]
-}
diff --git a/src/main/deploy/choreo/StoO.traj b/src/main/deploy/choreo/StoO.traj
index f9ca3948..5c00e2a5 100644
--- a/src/main/deploy/choreo/StoO.traj
+++ b/src/main/deploy/choreo/StoO.traj
@@ -4,8 +4,8 @@
  "snapshot":{
   "waypoints":[
     {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.6131027936935425, "y":1.1535886526107788, "heading":1.9909931005444836, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":1.6131027936935425, "y":1.1535886526107788, "heading":1.9909931005444836, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.5329699516296387, "y":0.654477596282959, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -16,8 +16,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.6131027936935425 m", "val":1.6131027936935425}, "y":{"exp":"1.1535886526107788 m", "val":1.1535886526107788}, "heading":{"exp":"1.9909931005444836 rad", "val":1.9909931005444836}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"1.6131027936935425 m", "val":1.6131027936935425}, "y":{"exp":"1.1535886526107788 m", "val":1.1535886526107788}, "heading":{"exp":"1.9909931005444836 rad", "val":1.9909931005444836}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"0.5329699516296387 m", "val":0.5329699516296387}, "y":{"exp":"0.654477596282959 m", "val":0.654477596282959}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -53,62 +53,65 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,0.8731,1.62816],
+  "waypoints":[0.0,0.87316,1.76723],
   "samples":[
-    {"t":0.0, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.10376, "ay":-5.52485, "alpha":-0.03459, "fx":[-84.72077,-84.39689,-84.04436,-84.36919], "fy":[-91.03195,-91.3333,-91.65688,-91.35696]},
-    {"t":0.0291, "x":2.40232, "y":1.99766, "heading":-3.14151, "vx":-0.14854, "vy":-0.16079, "omega":-0.00101, "ax":-5.10221, "ay":-5.52317, "alpha":-0.03904, "fx":[-84.73841,-84.37317,-83.97524,-84.3417], "fy":[-90.96396,-91.30418,-91.66906,-91.33065]},
-    {"t":0.05821, "x":2.39584, "y":1.99064, "heading":-3.14154, "vx":-0.29703, "vy":-0.32153, "omega":-0.00214, "ax":-5.0999, "ay":-5.52066, "alpha":-0.04567, "fx":[-84.76466,-84.33784,-83.87217,-84.30064], "fy":[-90.86254,-91.26081,-91.68717,-91.29138]},
-    {"t":0.08731, "x":2.38503, "y":1.97895, "heading":3.14159, "vx":-0.44545, "vy":-0.4822, "omega":-0.00347, "ax":-5.09607, "ay":-5.51651, "alpha":-0.05664, "fx":[-84.80785,-84.27963,-83.70194,-84.2327], "fy":[-90.69502,-91.1893,-91.71689,-91.22642]},
-    {"t":0.11641, "x":2.36991, "y":1.96258, "heading":3.14148, "vx":-0.59376, "vy":-0.64275, "omega":-0.00512, "ax":-5.08853, "ay":-5.50835, "alpha":-0.07822, "fx":[-84.89218,-84.16562,-83.36731,-84.09867], "fy":[-90.36556,-91.04921,-91.77473,-91.0983]},
-    {"t":0.14552, "x":2.35047, "y":1.94154, "heading":3.14134, "vx":-0.74186, "vy":-0.80306, "omega":-0.0074, "ax":-5.06686, "ay":-5.48487, "alpha":-0.14021, "fx":[-85.12967,-83.84172,-82.40806,-83.71113], "fy":[-89.42012,-90.65129,-91.93612,-90.7278]},
-    {"t":0.17462, "x":2.32674, "y":1.91584, "heading":3.14112, "vx":-0.88932, "vy":-0.96269, "omega":-0.01148, "ax":-4.4031, "ay":-4.76668, "alpha":-1.97319, "fx":[-89.21431,-76.12615,-55.64594,-70.20744], "fy":[-62.43516,-81.2333,-93.87683,-77.69326]},
-    {"t":0.20372, "x":2.29899, "y":1.88581, "heading":3.14079, "vx":-1.01747, "vy":-1.10142, "omega":-0.0689, "ax":-0.00096, "ay":-0.00086, "alpha":-9.7736, "fx":[-46.08987,-46.15639,46.05032,46.13246], "fy":[46.12587,-46.07996,-46.16299,46.06024]},
-    {"t":0.23283, "x":2.26938, "y":1.85375, "heading":3.13878, "vx":-1.01749, "vy":-1.10144, "omega":-0.35335, "ax":-0.00001, "ay":0.00001, "alpha":-9.23854, "fx":[-43.46061,-43.70578,43.46024,43.7052], "fy":[43.70565,-43.46015,-43.70533,43.4607]},
-    {"t":0.26193, "x":2.23977, "y":1.8217, "heading":3.1285, "vx":-1.01749, "vy":-1.10144, "omega":-0.62222, "ax":0.0, "ay":0.0, "alpha":-8.67153, "fx":[-40.36908,-41.44046,40.36903,41.44038], "fy":[41.44045,-40.36902,-41.4404,40.36909]},
-    {"t":0.29103, "x":2.21015, "y":1.78964, "heading":3.11039, "vx":-1.01749, "vy":-1.10144, "omega":-0.87459, "ax":0.0, "ay":0.0, "alpha":-8.07258, "fx":[-36.87601,-39.25228,36.87603,39.2523], "fy":[39.25229,-36.87602,-39.25229,36.87602]},
-    {"t":0.32014, "x":2.18054, "y":1.75758, "heading":3.08494, "vx":-1.01749, "vy":-1.10144, "omega":-1.10953, "ax":0.0, "ay":0.0, "alpha":-7.44244, "fx":[-33.06544,-37.04179,33.06549,37.04185], "fy":[37.04181,-33.06548,-37.04183,33.06544]},
-    {"t":0.34924, "x":2.15093, "y":1.72553, "heading":3.05264, "vx":-1.01749, "vy":-1.10144, "omega":-1.32613, "ax":0.0, "ay":0.0, "alpha":-6.78267, "fx":[-29.02856,-34.71332,29.02865,34.71342], "fy":[34.71334,-29.02865,-34.7134,29.02856]},
-    {"t":0.37834, "x":2.12132, "y":1.69347, "heading":3.01405, "vx":-1.01749, "vy":-1.10144, "omega":-1.52353, "ax":0.0, "ay":0.0, "alpha":-6.09563, "fx":[-24.86495,-32.18043,24.86509,32.1806], "fy":[32.18046,-24.8651,-32.18057,24.86494]},
-    {"t":0.40745, "x":2.0917, "y":1.66142, "heading":2.96971, "vx":-1.01749, "vy":-1.10144, "omega":-1.70093, "ax":0.00001, "ay":-0.00001, "alpha":-5.3844, "fx":[-20.68215,-29.37125,20.68236,29.37148], "fy":[29.37128,-20.68236,-29.37145,20.68214]},
-    {"t":0.43655, "x":2.06209, "y":1.62936, "heading":2.92021, "vx":-1.01749, "vy":-1.10144, "omega":-1.85764, "ax":0.00001, "ay":-0.00001, "alpha":-4.6526, "fx":[-16.59341,-26.23252,16.59367,26.23279], "fy":[26.23255,-16.59367,-26.23276,16.59341]},
-    {"t":0.46565, "x":2.03248, "y":1.59731, "heading":2.86614, "vx":-1.01749, "vy":-1.10144, "omega":-1.99304, "ax":0.00001, "ay":-0.00001, "alpha":-3.90401, "fx":[-12.71377,-22.73203,12.71405,22.73232], "fy":[22.73206,-12.71404,-22.73229,12.71378]},
-    {"t":0.49476, "x":2.00287, "y":1.56525, "heading":2.80814, "vx":-1.01749, "vy":-1.10144, "omega":-2.10666, "ax":0.00001, "ay":-0.00001, "alpha":-3.14246, "fx":[-9.15572,-18.86018,9.15595,18.86041], "fy":[18.8602,-9.15594,-18.86039,9.15573]},
-    {"t":0.52386, "x":1.97325, "y":1.53319, "heading":2.74683, "vx":-1.01749, "vy":-1.10144, "omega":-2.19812, "ax":0.0, "ay":0.0, "alpha":-2.37127, "fx":[-6.02387,-14.62834,6.02396,14.62844], "fy":[14.62835,-6.02395,-14.62843,6.02387]},
-    {"t":0.55296, "x":1.94364, "y":1.50114, "heading":2.68286, "vx":-1.01749, "vy":-1.10144, "omega":-2.26713, "ax":0.0, "ay":0.0, "alpha":-1.59344, "fx":[-3.41129,-10.0687,3.4112,10.06861], "fy":[10.0687,-3.41121,-10.06861,3.41129]},
-    {"t":0.58207, "x":1.91403, "y":1.46908, "heading":2.61688, "vx":-1.01749, "vy":-1.10144, "omega":-2.3135, "ax":-0.00001, "ay":0.00001, "alpha":-0.8113, "fx":[-1.3952,-5.22997,1.39492,5.22969], "fy":[5.22996,-1.39493,-5.2297,1.39519]},
-    {"t":0.61117, "x":1.88442, "y":1.43703, "heading":2.54954, "vx":-1.01749, "vy":-1.10144, "omega":-2.33712, "ax":-0.00001, "ay":0.00001, "alpha":-0.02697, "fx":[-0.03479,-0.1768,0.03436,0.17637], "fy":[0.17678,-0.03438,-0.17639,0.03477]},
-    {"t":0.64027, "x":1.8548, "y":1.40497, "heading":2.48153, "vx":-1.01749, "vy":-1.10144, "omega":-2.3379, "ax":-0.00002, "ay":0.00002, "alpha":0.75775, "fx":[0.63165,5.01548,-0.63225,-5.01607], "fy":[-5.0155,0.63223,5.01605,-0.63168]},
-    {"t":0.66938, "x":1.82519, "y":1.37292, "heading":2.41349, "vx":-1.01749, "vy":-1.10144, "omega":-2.31585, "ax":-0.00005, "ay":0.00004, "alpha":1.54086, "fx":[0.58783,10.26235,-0.58945,-10.26395], "fy":[-10.26242,0.58938,10.26388,-0.58789]},
-    {"t":0.69848, "x":1.79558, "y":1.34086, "heading":2.34609, "vx":-1.01749, "vy":-1.10144, "omega":-2.271, "ax":-0.00035, "ay":0.00032, "alpha":2.32016, "fx":[-0.16229,15.4727,0.1506,-15.48403], "fy":[-15.47313,-0.15105,15.4836,0.16184]},
-    {"t":0.72758, "x":1.76597, "y":1.3088, "heading":2.27999, "vx":-1.0175, "vy":-1.10143, "omega":-2.20348, "ax":-0.00315, "ay":0.00291, "alpha":3.09297, "fx":[-1.62466,20.52467,1.51712,-20.62573], "fy":[-20.52812,-1.52161,20.62229,1.62015]},
-    {"t":0.75669, "x":1.73635, "y":1.27675, "heading":2.21587, "vx":-1.0176, "vy":-1.10135, "omega":-2.11346, "ax":-0.02849, "ay":0.02634, "alpha":3.85602, "fx":[-4.09439,25.02633,3.1035,-25.91933], "fy":[-25.04987,-3.14977,25.89624,4.04533]},
-    {"t":0.78579, "x":1.70673, "y":1.24471, "heading":2.15436, "vx":-1.01843, "vy":-1.10058, "omega":-2.00124, "ax":-0.24876, "ay":0.23173, "alpha":4.58871, "fx":[-10.63174,26.18766,1.76741,-33.775], "fy":[-26.28888,-2.12728,33.71888,10.02229]},
-    {"t":0.81489, "x":1.67698, "y":1.21278, "heading":2.09611, "vx":-1.02567, "vy":-1.09384, "omega":-1.86769, "ax":-1.88252, "ay":1.85844, "alpha":4.24043, "fx":[-43.8043,-2.39918,-22.58399,-55.71062], "fy":[1.67396,27.93544,57.80486,35.49116]},
-    {"t":0.844, "x":1.64633, "y":1.18173, "heading":2.04176, "vx":-1.08045, "vy":-1.03975, "omega":-1.74428, "ax":-4.21636, "ay":5.00477, "alpha":0.92918, "fx":[-76.50689,-64.19914,-63.4634,-74.67412], "fy":[75.85539,85.7956,88.98499,80.34806]},
-    {"t":0.8731, "x":1.6131, "y":1.15359, "heading":1.99099, "vx":-1.20316, "vy":-0.89409, "omega":-1.71724, "ax":-3.50578, "ay":5.46433, "alpha":1.01765, "fx":[-65.80325,-50.87052,-50.94855,-64.22792], "fy":[83.99578,93.28598,96.0298,88.06493]},
-    {"t":0.90593, "x":1.57172, "y":1.12718, "heading":1.93462, "vx":-1.31825, "vy":-0.71471, "omega":-1.68383, "ax":-0.88214, "ay":1.71212, "alpha":5.14988, "fx":[-33.33245,19.77884,-0.67343,-44.11192], "fy":[-3.20684,17.30054,58.33755,40.79766]},
-    {"t":0.93876, "x":1.52796, "y":1.10464, "heading":1.87934, "vx":-1.34721, "vy":-0.6585, "omega":-1.51477, "ax":-0.08015, "ay":0.16481, "alpha":5.37733, "fx":[-18.06135,30.80225,14.91478,-32.95634], "fy":[-29.18363,-13.67837,34.55081,19.2108]},
-    {"t":0.97159, "x":1.48369, "y":1.08311, "heading":1.82961, "vx":-1.34984, "vy":-0.65309, "omega":-1.33824, "ax":-0.00715, "ay":0.01479, "alpha":4.98193, "fx":[-16.84227,28.63586,16.56709,-28.83376], "fy":[-28.49148,-16.4585,28.97798,16.95021]},
-    {"t":1.00441, "x":1.43938, "y":1.06168, "heading":1.78568, "vx":-1.35008, "vy":-0.6526, "omega":-1.17469, "ax":-0.00063, "ay":0.0013, "alpha":4.59851, "fx":[-16.58073,25.81161,16.55708,-25.82946], "fy":[-25.79904,-16.54747,25.84203,16.59034]},
-    {"t":1.03724, "x":1.39505, "y":1.04026, "heading":1.74712, "vx":-1.3501, "vy":-0.65256, "omega":-1.02372, "ax":-0.00005, "ay":0.00011, "alpha":4.23277, "fx":[-16.15705,23.16043,16.15505,-23.16201], "fy":[-23.15937,-16.15422,23.16307,16.15788]},
-    {"t":1.07007, "x":1.35073, "y":1.01883, "heading":1.71351, "vx":-1.3501, "vy":-0.65256, "omega":-0.88477, "ax":0.0, "ay":0.00001, "alpha":3.88449, "fx":[-15.53263,20.74523,15.53247,-20.74536], "fy":[-20.74514,-15.5324,20.74545,15.5327]},
-    {"t":1.1029, "x":1.30641, "y":0.99741, "heading":1.68446, "vx":-1.3501, "vy":-0.65256, "omega":-0.75724, "ax":0.0, "ay":0.0, "alpha":3.55327, "fx":[-14.75325,18.55576,14.75324,-18.55578], "fy":[-18.55576,-14.75324,18.55578,14.75326]},
-    {"t":1.13573, "x":1.26209, "y":0.97599, "heading":1.6596, "vx":-1.3501, "vy":-0.65256, "omega":-0.64059, "ax":0.0, "ay":0.0, "alpha":3.23854, "fx":[-13.86271,16.57273,13.86271,-16.57273], "fy":[-16.57273,-13.86271,16.57273,13.86271]},
-    {"t":1.16856, "x":1.21776, "y":0.95457, "heading":1.63857, "vx":-1.3501, "vy":-0.65256, "omega":-0.53428, "ax":0.0, "ay":0.0, "alpha":2.93959, "fx":[-12.89659,14.77498,12.89659,-14.77498], "fy":[-14.77498,-12.89659,14.77498,12.89659]},
-    {"t":1.20139, "x":1.17344, "y":0.93314, "heading":1.62103, "vx":-1.3501, "vy":-0.65256, "omega":-0.43777, "ax":0.0, "ay":0.0, "alpha":2.6556, "fx":[-11.88297,13.14119,11.88297,-13.14119], "fy":[-13.14119,-11.88297,13.14119,11.88297]},
-    {"t":1.23422, "x":1.12912, "y":0.91172, "heading":1.60666, "vx":-1.3501, "vy":-0.65256, "omega":-0.35059, "ax":0.0, "ay":0.0, "alpha":2.38564, "fx":[-10.84355,11.65068,10.84355,-11.65068], "fy":[-11.65068,-10.84355,11.65068,10.84355]},
-    {"t":1.26705, "x":1.0848, "y":0.8903, "heading":1.59515, "vx":-1.3501, "vy":-0.65256, "omega":-0.27227, "ax":0.0, "ay":0.0, "alpha":2.12872, "fx":[-9.79475,10.2839,9.79475,-10.2839], "fy":[-10.2839,-9.79475,10.2839,9.79475]},
-    {"t":1.29987, "x":1.04048, "y":0.86887, "heading":1.58621, "vx":-1.3501, "vy":-0.65256, "omega":-0.20239, "ax":0.0, "ay":0.0, "alpha":1.88376, "fx":[-8.74864,9.02267,8.74864,-9.02267], "fy":[-9.02267,-8.74864,9.02267,8.74864]},
-    {"t":1.3327, "x":0.99615, "y":0.84745, "heading":1.57957, "vx":-1.3501, "vy":-0.65256, "omega":-0.14055, "ax":0.0, "ay":0.0, "alpha":1.64966, "fx":[-7.71376,7.85032,7.71376,-7.85032], "fy":[-7.85032,-7.71376,7.85032,7.71376]},
-    {"t":1.36553, "x":0.95183, "y":0.82603, "heading":1.57496, "vx":-1.3501, "vy":-0.65256, "omega":-0.08639, "ax":0.00002, "ay":0.00001, "alpha":1.42528, "fx":[-6.69551,6.75198,6.69603,-6.75145], "fy":[-6.75158,-6.69564,6.75184,6.6959]},
-    {"t":1.39836, "x":0.90751, "y":0.80461, "heading":1.57212, "vx":-1.3501, "vy":-0.65256, "omega":-0.0396, "ax":0.55769, "ay":0.26955, "alpha":1.19326, "fx":[3.63455,14.88035,14.77167,3.59565], "fy":[-1.21198,-1.18075,10.0501,10.16927]},
-    {"t":1.43119, "x":0.86349, "y":0.78333, "heading":1.57082, "vx":-1.33179, "vy":-0.64371, "omega":-0.00043, "ax":6.73047, "ay":3.2531, "alpha":0.00771, "fx":[111.29183,111.32001,111.26394,111.23566], "fy":[53.75443,53.69835,53.81553,53.87168]},
-    {"t":1.46402, "x":0.82339, "y":0.76395, "heading":1.57081, "vx":-1.11084, "vy":-0.53691, "omega":-0.00018, "ax":6.75662, "ay":3.26574, "alpha":0.00301, "fx":[111.7158,111.7267,111.70459,111.69367], "fy":[53.98203,53.95991,54.00589,54.02801]},
-    {"t":1.49685, "x":0.79057, "y":0.74808, "heading":1.5708, "vx":-0.88903, "vy":-0.4297, "omega":-0.00008, "ax":6.76525, "ay":3.26991, "alpha":0.00147, "fx":[111.85563,111.86092,111.85011,111.84481], "fy":[54.0571,54.04629,54.06874,54.07956]},
-    {"t":1.52968, "x":0.76503, "y":0.73574, "heading":1.5708, "vx":-0.66693, "vy":-0.32236, "omega":-0.00003, "ax":6.76955, "ay":3.27199, "alpha":0.0007, "fx":[111.92524,111.92776,111.92259,111.92006], "fy":[54.09448,54.08931,54.10004,54.10521]},
-    {"t":1.5625, "x":0.74678, "y":0.72692, "heading":1.5708, "vx":-0.4447, "vy":-0.21494, "omega":-0.00001, "ax":6.77212, "ay":3.27323, "alpha":0.00024, "fx":[111.9669,111.96777,111.96598,111.96511], "fy":[54.11686,54.11506,54.11878,54.12057]},
-    {"t":1.59533, "x":0.73583, "y":0.72163, "heading":1.5708, "vx":-0.22238, "vy":-0.10748, "omega":0.0, "ax":6.77383, "ay":3.27406, "alpha":-0.00006, "fx":[111.99463,111.9944,111.99487,111.99509], "fy":[54.13175,54.13221,54.13125,54.13079]},
-    {"t":1.62816, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.09969, "ay":-5.52861, "alpha":-0.03468, "fx":[-84.65454,-84.32988,-83.97598,-84.30159], "fy":[-91.09356,-91.3952,-91.71956,-91.41937]},
+    {"t":0.02911, "x":2.40232, "y":1.99766, "heading":-3.14151, "vx":-0.14843, "vy":-0.16091, "omega":-0.00101, "ax":-5.09814, "ay":-5.52693, "alpha":-0.03913, "fx":[-84.67233,-84.30621,-83.90676,-84.27408], "fy":[-91.0255,-91.36604,-91.73178,-91.39307]},
+    {"t":0.05821, "x":2.39584, "y":1.99063, "heading":-3.14154, "vx":-0.29681, "vy":-0.32177, "omega":-0.00215, "ax":-5.09583, "ay":-5.52441, "alpha":-0.04579, "fx":[-84.69879,-84.27098,-83.80353,-84.233], "fy":[-90.92397,-91.32259,-91.74994,-91.35382]},
+    {"t":0.08732, "x":2.38504, "y":1.97893, "heading":3.14159, "vx":-0.44513, "vy":-0.48256, "omega":-0.00348, "ax":-5.092, "ay":-5.52026, "alpha":-0.05678, "fx":[-84.74235,-84.21291,-83.63304,-84.16501], "fy":[-90.75626,-91.25096,-91.77975,-91.28889]},
+    {"t":0.11642, "x":2.36993, "y":1.96255, "heading":3.14148, "vx":-0.59333, "vy":-0.64323, "omega":-0.00513, "ax":-5.08447, "ay":-5.5121, "alpha":-0.0784, "fx":[-84.82738,-84.09917,-83.29788,-84.0309], "fy":[-90.4264,-91.11062,-91.83777,-91.16082]},
+    {"t":0.14553, "x":2.35051, "y":1.94149, "heading":3.14134, "vx":-0.74131, "vy":-0.80366, "omega":-0.00742, "ax":-5.06281, "ay":-5.4886, "alpha":-0.14055, "fx":[-85.06692,-83.77602,-82.33703,-83.64305], "fy":[-89.4797,-90.71197,-91.99966,-90.79042]},
+    {"t":0.17463, "x":2.32679, "y":1.91577, "heading":3.14112, "vx":-0.88867, "vy":-0.96341, "omega":-0.01151, "ax":-4.39735, "ay":-4.76751, "alpha":-1.98283, "fx":[-89.2032,-76.0635,-55.46775,-70.07908], "fy":[-62.38158,-81.25992,-93.94298,-77.70875]},
+    {"t":0.20374, "x":2.29906, "y":1.88571, "heading":3.14078, "vx":-1.01665, "vy":-1.10217, "omega":-0.06922, "ax":-0.00096, "ay":-0.00085, "alpha":-9.78551, "fx":[-46.1459,-46.21275,46.10639,46.1888], "fy":[46.1823,-46.13592,-46.21925,46.11637]},
+    {"t":0.23284, "x":2.26947, "y":1.85364, "heading":3.13877, "vx":-1.01668, "vy":-1.10219, "omega":-0.35403, "ax":-0.00001, "ay":0.00001, "alpha":-9.24982, "fx":[-43.51319,-43.75965,43.5128,43.75905], "fy":[43.75952,-43.51271,-43.75919,43.51328]},
+    {"t":0.26195, "x":2.23988, "y":1.82156, "heading":3.12847, "vx":-1.01668, "vy":-1.10219, "omega":-0.62324, "ax":0.0, "ay":0.0, "alpha":-8.68192, "fx":[-40.41612,-41.49138,40.41607,41.49131], "fy":[41.49137,-40.41606,-41.49132,40.41614]},
+    {"t":0.29105, "x":2.21029, "y":1.78948, "heading":3.11033, "vx":-1.01668, "vy":-1.10219, "omega":-0.87593, "ax":0.0, "ay":0.0, "alpha":-8.08177, "fx":[-36.91553,-39.29928,36.91555,39.29929], "fy":[39.29929,-36.91554,-39.29929,36.91554]},
+    {"t":0.32016, "x":2.1807, "y":1.7574, "heading":3.08483, "vx":-1.01668, "vy":-1.10219, "omega":-1.11115, "ax":0.0, "ay":0.0, "alpha":-7.4501, "fx":[-33.09564,-37.08333,33.09569,37.08338], "fy":[37.08334,-33.09569,-37.08337,33.09564]},
+    {"t":0.34926, "x":2.15111, "y":1.72532, "heading":3.05249, "vx":-1.01668, "vy":-1.10219, "omega":-1.32799, "ax":0.0, "ay":0.0, "alpha":-6.78843, "fx":[-29.04792,-34.74722,29.04801,34.74733], "fy":[34.74724,-29.04801,-34.74731,29.04792]},
+    {"t":0.37837, "x":2.12152, "y":1.69324, "heading":3.01384, "vx":-1.01668, "vy":-1.10219, "omega":-1.52557, "ax":0.0, "ay":0.0, "alpha":-6.09911, "fx":[-24.87241,-32.20401,24.87256,32.20417], "fy":[32.20403,-24.87256,-32.20415,24.87241]},
+    {"t":0.40747, "x":2.09193, "y":1.66116, "heading":2.96944, "vx":-1.01668, "vy":-1.10219, "omega":-1.70309, "ax":0.00001, "ay":-0.00001, "alpha":-5.38522, "fx":[-20.6773,-29.38129,20.67751,29.38152], "fy":[29.38132,-20.67751,-29.38149,20.6773]},
+    {"t":0.43658, "x":2.06234, "y":1.62908, "heading":2.91987, "vx":-1.01668, "vy":-1.10219, "omega":-1.85982, "ax":0.00001, "ay":-0.00001, "alpha":-4.65035, "fx":[-16.57657,-26.22546,16.57684,26.22574], "fy":[26.22549,-16.57683,-26.22571,16.57657]},
+    {"t":0.46568, "x":2.03275, "y":1.597, "heading":2.86574, "vx":-1.01668, "vy":-1.10219, "omega":-1.99517, "ax":0.00001, "ay":-0.00001, "alpha":-3.89837, "fx":[-12.68623,-22.70432,12.68651,22.70461], "fy":[22.70435,-12.6865,-22.70458,12.68624]},
+    {"t":0.49479, "x":2.00316, "y":1.56492, "heading":2.80767, "vx":-1.01668, "vy":-1.10219, "omega":-2.10864, "ax":0.00001, "ay":-0.00001, "alpha":-3.13308, "fx":[-9.11956,-18.80817,9.11978,18.80839], "fy":[18.80819,-9.11977,-18.80838,9.11957]},
+    {"t":0.52389, "x":1.97357, "y":1.53284, "heading":2.7463, "vx":-1.01668, "vy":-1.10219, "omega":-2.19982, "ax":0.0, "ay":0.0, "alpha":-2.35792, "fx":[-5.98222,-14.54916,5.98231,14.54925], "fy":[14.54917,-5.98231,-14.54924,5.98223]},
+    {"t":0.553, "x":1.94397, "y":1.50076, "heading":2.68227, "vx":-1.01668, "vy":-1.10219, "omega":-2.26845, "ax":0.0, "ay":0.0, "alpha":-1.57588, "fx":[-3.36786,-9.95967,3.36777,9.95958], "fy":[9.95967,-3.36777,-9.95958,3.36786]},
+    {"t":0.5821, "x":1.91438, "y":1.46868, "heading":2.61625, "vx":-1.01668, "vy":-1.10219, "omega":-2.31432, "ax":-0.00001, "ay":0.00001, "alpha":-0.78944, "fx":[-1.35441,-5.08988,1.35412,5.08959], "fy":[5.08987,-1.35413,-5.0896,1.3544]},
+    {"t":0.61121, "x":1.88479, "y":1.4366, "heading":2.54889, "vx":-1.01668, "vy":-1.10219, "omega":-2.33729, "ax":-0.00001, "ay":0.00001, "alpha":-0.00073, "fx":[-0.00115,-0.00499,0.00071,0.00456], "fy":[0.00498,-0.00073,-0.00457,0.00113]},
+    {"t":0.64031, "x":1.8552, "y":1.40452, "heading":2.48086, "vx":-1.01668, "vy":-1.10219, "omega":-2.33732, "ax":-0.00002, "ay":0.00002, "alpha":0.78828, "fx":[0.65362,5.21795,-0.65425,-5.21857], "fy":[-5.21798,0.65422,5.21855,-0.65365]},
+    {"t":0.66942, "x":1.82561, "y":1.37244, "heading":2.41283, "vx":-1.01668, "vy":-1.10219, "omega":-2.31437, "ax":-0.00006, "ay":0.00005, "alpha":1.57552, "fx":[0.59407,10.49346,-0.59597,-10.49533], "fy":[-10.49354,0.5959,10.49526,-0.59415]},
+    {"t":0.69852, "x":1.79602, "y":1.34036, "heading":2.34547, "vx":-1.01668, "vy":-1.10219, "omega":-2.26852, "ax":-0.00043, "ay":0.0004, "alpha":2.35866, "fx":[-0.17592,15.72816,0.16151,-15.7421], "fy":[-15.7287,-0.16208,15.74156,0.17535]},
+    {"t":0.72763, "x":1.76643, "y":1.30828, "heading":2.27945, "vx":-1.0167, "vy":-1.10218, "omega":-2.19987, "ax":-0.0039, "ay":0.0036, "alpha":3.13494, "fx":[-1.6701,20.79109,1.53712,-20.91585], "fy":[-20.79541,-1.54277,20.91153,1.66441]},
+    {"t":0.75673, "x":1.73684, "y":1.27621, "heading":2.21542, "vx":-1.01681, "vy":-1.10207, "omega":-2.10862, "ax":-0.03515, "ay":0.03246, "alpha":3.90085, "fx":[-4.26464,25.21708,3.04063,-26.31755], "fy":[-25.24647,-3.0984,26.28886,4.20263]},
+    {"t":0.78584, "x":1.70723, "y":1.24414, "heading":2.15405, "vx":-1.01783, "vy":-1.10113, "omega":-1.99509, "ax":-0.30589, "ay":0.28507, "alpha":4.6268, "fx":[-11.74897,25.56423,0.83611,-34.88095], "fy":[-25.68936,-1.24339,34.82408,10.96126]},
+    {"t":0.81495, "x":1.67747, "y":1.21222, "heading":2.09598, "vx":-1.02674, "vy":-1.09283, "omega":-1.86043, "ax":-2.20543, "ay":2.20154, "alpha":3.90645, "fx":[-49.45459,-10.13058,-27.55105,-58.71696], "fy":[9.10815,35.32272,61.38388,39.78121]},
+    {"t":0.84405, "x":1.64666, "y":1.18134, "heading":2.04183, "vx":-1.09092, "vy":-1.02876, "omega":-1.74673, "ax":-4.25511, "ay":5.16872, "alpha":0.78985, "fx":[-76.30725,-65.62335,-64.81949,-74.65665], "fy":[79.69635,88.21183,90.72357,83.19501]},
+    {"t":0.87316, "x":1.6131, "y":1.15359, "heading":1.99099, "vx":-1.21477, "vy":-0.87832, "omega":-1.72374, "ax":-3.47156, "ay":5.63195, "alpha":0.93653, "fx":[-64.77245,-50.69479,-50.76852,-63.35141], "fy":[87.47832,95.96297,98.18447,90.83651]},
+    {"t":0.90754, "x":1.56928, "y":1.12672, "heading":1.93172, "vx":-1.33415, "vy":-0.68465, "omega":-1.69153, "ax":-0.83576, "ay":1.72032, "alpha":5.34907, "fx":[-33.42472,21.99252,0.61315,-44.45282], "fy":[-4.20914,16.92181,59.45762,41.60101]},
+    {"t":0.94193, "x":1.52291, "y":1.10419, "heading":1.87355, "vx":-1.36289, "vy":-0.62549, "omega":-1.50759, "ax":-0.06629, "ay":0.14514, "alpha":5.54249, "fx":[-18.50388,31.89174,15.85395,-33.62587], "fy":[-30.38949,-14.7044,35.10662,19.58573]},
+    {"t":0.97632, "x":1.476, "y":1.08277, "heading":1.82171, "vx":-1.36517, "vy":-0.6205, "omega":-1.317, "ax":-0.00518, "ay":0.0114, "alpha":5.09939, "fx":[-17.43145,29.20598,17.22933,-29.34648], "fy":[-29.08883,-17.14052,29.46354,17.51988]},
+    {"t":1.0107, "x":1.42905, "y":1.06144, "heading":1.77642, "vx":-1.36535, "vy":-0.62011, "omega":-1.14165, "ax":-0.0004, "ay":0.00087, "alpha":4.669, "fx":[-17.07243,26.05385,17.05733,-26.06498], "fy":[-26.04497,-17.05046,26.07387,17.0793]},
+    {"t":1.04509, "x":1.3821, "y":1.04011, "heading":1.73716, "vx":-1.36536, "vy":-0.62008, "omega":-0.98109, "ax":-0.00003, "ay":0.00007, "alpha":4.25594, "fx":[-16.47604,23.12473,16.47493,-23.12559], "fy":[-23.12408,-16.47442,23.12624,16.47656]},
+    {"t":1.07948, "x":1.33515, "y":1.01879, "heading":1.70343, "vx":-1.36536, "vy":-0.62008, "omega":-0.83474, "ax":0.0, "ay":0.0, "alpha":3.86012, "fx":[-15.64223,20.45842,15.64215,-20.45849], "fy":[-20.45838,-15.64211,20.45853,15.64226]},
+    {"t":1.11387, "x":1.2882, "y":0.99747, "heading":1.67472, "vx":-1.36536, "vy":-0.62008, "omega":-0.702, "ax":0.0, "ay":0.0, "alpha":3.48119, "fx":[-14.63038,18.03766,14.63037,-18.03766], "fy":[-18.03766,-14.63037,18.03766,14.63038]},
+    {"t":1.14825, "x":1.24125, "y":0.97614, "heading":1.65058, "vx":-1.36536, "vy":-0.62008, "omega":-0.5823, "ax":0.0, "ay":0.0, "alpha":3.11855, "fx":[-13.49252,15.83758,13.49252,-15.83758], "fy":[-15.83758,-13.49252,15.83758,13.49252]},
+    {"t":1.18264, "x":1.1943, "y":0.95482, "heading":1.63056, "vx":-1.36536, "vy":-0.62008, "omega":-0.47506, "ax":0.0, "ay":0.0, "alpha":2.77139, "fx":[-12.26992,13.83163,12.26992,-13.83163], "fy":[-13.83163,-12.26992,13.83163,12.26992]},
+    {"t":1.21703, "x":1.14735, "y":0.9335, "heading":1.61422, "vx":-1.36536, "vy":-0.62008, "omega":-0.37976, "ax":0.0, "ay":0.0, "alpha":2.43871, "fx":[-10.99442,11.99329,10.99442,-11.99329], "fy":[-11.99329,-10.99442,11.99329,10.99442]},
+    {"t":1.25142, "x":1.1004, "y":0.91217, "heading":1.60116, "vx":-1.36536, "vy":-0.62008, "omega":-0.2959, "ax":0.0, "ay":0.0, "alpha":2.11936, "fx":[-9.68997,10.29709,9.68997,-10.29709], "fy":[-10.29709,-9.68997,10.29709,9.68997]},
+    {"t":1.2858, "x":1.05344, "y":0.89085, "heading":1.59099, "vx":-1.36536, "vy":-0.62008, "omega":-0.22302, "ax":0.0, "ay":0.0, "alpha":1.81203, "fx":[-8.37397,8.71914,8.37397,-8.71914], "fy":[-8.71914,-8.37397,8.71914,8.37397]},
+    {"t":1.32019, "x":1.00649, "y":0.86953, "heading":1.58332, "vx":-1.36536, "vy":-0.62008, "omega":-0.16071, "ax":0.0, "ay":0.0, "alpha":1.51531, "fx":[-7.05843,7.23745,7.05843,-7.23745], "fy":[-7.23745,-7.05843,7.23745,7.05843]},
+    {"t":1.35458, "x":0.95954, "y":0.84821, "heading":1.57779, "vx":-1.36536, "vy":-0.62008, "omega":-0.1086, "ax":0.0, "ay":0.0, "alpha":1.22769, "fx":[-5.75103,5.83206,5.75103,-5.83206], "fy":[-5.83206,-5.75103,5.83206,5.75103]},
+    {"t":1.38897, "x":0.91259, "y":0.82688, "heading":1.57406, "vx":-1.36536, "vy":-0.62008, "omega":-0.06638, "ax":0.0, "ay":0.0, "alpha":0.94762, "fx":[-4.45584,4.485,4.45584,-4.485], "fy":[-4.485,-4.45584,4.485,4.45584]},
+    {"t":1.42335, "x":0.86564, "y":0.80556, "heading":1.57177, "vx":-1.36536, "vy":-0.62008, "omega":-0.0338, "ax":0.0, "ay":0.0, "alpha":0.67347, "fx":[-3.174,3.18021,3.174,-3.18021], "fy":[-3.18021,-3.174,3.18021,3.174]},
+    {"t":1.45774, "x":0.81869, "y":0.78424, "heading":1.57061, "vx":-1.36536, "vy":-0.62008, "omega":-0.01064, "ax":0.0, "ay":0.0, "alpha":0.40357, "fx":[-1.9042,1.9035,1.9042,-1.9035], "fy":[-1.9035,-1.9042,1.9035,1.9042]},
+    {"t":1.49213, "x":0.77174, "y":0.76291, "heading":1.57025, "vx":-1.36536, "vy":-0.62008, "omega":0.00324, "ax":0.0, "ay":0.0, "alpha":0.13624, "fx":[-0.64308,0.64238,0.64308,-0.64238], "fy":[-0.64238,-0.64308,0.64238,0.64308]},
+    {"t":1.52651, "x":0.72479, "y":0.74159, "heading":1.57036, "vx":-1.36536, "vy":-0.62008, "omega":0.00793, "ax":0.00036, "ay":0.00016, "alpha":-0.13021, "fx":[0.62044,-0.60812,-0.60866,0.6199], "fy":[0.61668,0.61722,-0.61133,-0.61187]},
+    {"t":1.5609, "x":0.67784, "y":0.72027, "heading":1.57063, "vx":-1.36535, "vy":-0.62007, "omega":0.00345, "ax":5.51296, "ay":2.50372, "alpha":-0.0855, "fx":[91.18553,90.71857,91.11408,91.5748], "fy":[41.77177,42.16797,41.01476,40.62572]},
+    {"t":1.59529, "x":0.63415, "y":0.70043, "heading":1.57075, "vx":-1.17577, "vy":-0.53398, "omega":0.00051, "ax":6.81458, "ay":3.09485, "alpha":-0.00431, "fx":[112.66047,112.6462,112.67645,112.69069], "fy":[51.1869,51.21716,51.15004,51.11981]},
+    {"t":1.62968, "x":0.59774, "y":0.68389, "heading":1.57077, "vx":-0.94144, "vy":-0.42755, "omega":0.00036, "ax":6.83636, "ay":3.10474, "alpha":-0.00307, "fx":[113.02283,113.0128,113.03446,113.04448], "fy":[51.34516,51.36682,51.31895,51.2973]},
+    {"t":1.66406, "x":0.56941, "y":0.67103, "heading":1.57078, "vx":-0.70635, "vy":-0.32079, "omega":0.00025, "ax":6.84378, "ay":3.10811, "alpha":-0.00264, "fx":[113.14626,113.13765,113.15636,113.16496], "fy":[51.39906,51.41777,51.37647,51.35776]},
+    {"t":1.69845, "x":0.54917, "y":0.66183, "heading":1.57079, "vx":-0.47101, "vy":-0.21391, "omega":0.00016, "ax":6.84752, "ay":3.10981, "alpha":-0.00243, "fx":[113.20847,113.20057,113.21779,113.22568], "fy":[51.42622,51.44344,51.40545,51.38824]},
+    {"t":1.73284, "x":0.53702, "y":0.65632, "heading":1.57079, "vx":-0.23555, "vy":-0.10697, "omega":0.00008, "ax":6.84978, "ay":3.11083, "alpha":-0.0023, "fx":[113.24595,113.23848,113.2548,113.26227], "fy":[51.44259,51.45891,51.42292,51.40661]},
+    {"t":1.76723, "x":0.53297, "y":0.65448, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]

From 771afcccfbaa90e4664a6acf00bb3c5fdc92044b Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 15:01:23 -0700
Subject: [PATCH 40/54] allow preclimb in auto lmfao

---
 src/main/java/frc/robot/Superstructure.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 0ca290c2..c47c6018 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -372,7 +372,9 @@ private void addTransitions() {
 
     bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate());
 
-    (preClimbReq.and(climbReq.negate()).and(() -> DriverStation.isTeleop()))
+    (preClimbReq.and(climbReq.negate())
+    // .and(() -> DriverStation.isTeleop())
+    )
         .onTrue(changeStateTo(SuperState.PRE_CLIMB));
 
     bindTransition(

From 1ed5129e7587a3fbcbcb8d88d4cf5b82bb91e2e5 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 15:27:56 -0700
Subject: [PATCH 41/54] Update at 'Sat Mar 14 15:27:55 PDT 2026'

---
 src/main/deploy/choreo/MRtoO.traj             |   4 +-
 src/main/deploy/choreo/OtoCR.traj             | 140 +++++++++---------
 src/main/deploy/choreo/OtoRR.traj             |  90 +++++------
 src/main/deploy/choreo/OtoS.traj              | 101 +++++++------
 src/main/deploy/choreo/PRtoO.traj             |  88 +++++------
 src/main/deploy/choreo/StoO.traj              | 126 ++++++++--------
 src/main/deploy/choreo/rebuiltChoreo.chor     |   8 +-
 src/main/java/frc/robot/Autos.java            |   3 +-
 src/main/java/frc/robot/Robot.java            |  12 +-
 src/main/java/frc/robot/Superstructure.java   |   9 +-
 .../subsystems/intake/SlapdownSubsystem.java  |   2 +-
 .../subsystems/swerve/SwerveSubsystem.java    |   8 +-
 12 files changed, 305 insertions(+), 286 deletions(-)

diff --git a/src/main/deploy/choreo/MRtoO.traj b/src/main/deploy/choreo/MRtoO.traj
index 0ddc0482..66055e26 100644
--- a/src/main/deploy/choreo/MRtoO.traj
+++ b/src/main/deploy/choreo/MRtoO.traj
@@ -17,8 +17,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"MR.x", "val":3.32966947555542}, "y":{"exp":"MR.y", "val":0.688210129737854}, "heading":{"exp":"MR.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"2.0304315090179443 m", "val":2.0304315090179443}, "y":{"exp":"0.749363362789154 m", "val":0.749363362789154}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"2.0304315090179443 m", "val":2.0304315090179443}, "y":{"exp":"0.749363362789154 m", "val":0.749363362789154}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
diff --git a/src/main/deploy/choreo/OtoCR.traj b/src/main/deploy/choreo/OtoCR.traj
index b3c63403..7d97a05b 100644
--- a/src/main/deploy/choreo/OtoCR.traj
+++ b/src/main/deploy/choreo/OtoCR.traj
@@ -3,7 +3,7 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":69, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":1.918229579925537, "y":2.544593811035156, "heading":-1.5737992594811685, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -14,7 +14,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":69, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.918229579925537 m", "val":1.918229579925537}, "y":{"exp":"2.5445938110351562 m", "val":2.544593811035156}, "heading":{"exp":"CR.heading", "val":-1.5737992594811685}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -51,72 +51,78 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,2.31226],
+  "waypoints":[0.0,2.62479],
   "samples":[
-    {"t":0.0, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.10002, "ay":6.30784, "alpha":0.01021, "fx":[67.76368,67.89908,67.81123,67.67586], "fy":[104.30559,104.21767,104.2751,104.36285]},
-    {"t":0.0367, "x":0.73494, "y":0.72411, "heading":1.5708, "vx":0.15048, "vy":0.23151, "omega":0.00037, "ax":4.09736, "ay":6.30374, "alpha":0.01427, "fx":[67.7102,67.89937,67.77674,67.58763], "fy":[104.24377,104.12099,104.20138,104.3238]},
-    {"t":0.07341, "x":0.74322, "y":0.73685, "heading":1.57081, "vx":0.30086, "vy":0.46288, "omega":0.0009, "ax":4.08943, "ay":6.29151, "alpha":0.02639, "fx":[67.55057,67.89998,67.67408,67.32486], "fy":[104.05914,103.83274,103.98204,104.20724]},
-    {"t":0.11011, "x":0.75702, "y":0.75808, "heading":1.57084, "vx":0.45096, "vy":0.69379, "omega":0.00187, "ax":2.55761, "ay":3.93519, "alpha":2.60714, "fx":[31.41257,61.50266,51.61639,24.61302], "fy":[59.48868,49.98538,71.41775,79.35702]},
-    {"t":0.14681, "x":0.7753, "y":0.7862, "heading":1.57091, "vx":0.54483, "vy":0.83822, "omega":0.09756, "ax":0.00013, "ay":-0.00001, "alpha":5.28428, "fx":[-24.92379,24.93372,24.92799,-24.92956], "fy":[-24.93192,-24.92583,24.93137,24.92595]},
-    {"t":0.18351, "x":0.79529, "y":0.81696, "heading":1.57449, "vx":0.54483, "vy":0.83822, "omega":0.2915, "ax":0.0, "ay":0.0, "alpha":4.90794, "fx":[-23.06761,23.23886,23.06771,-23.23877], "fy":[-23.23884,-23.06769,23.23878,23.06763]},
-    {"t":0.22022, "x":0.81529, "y":0.84772, "heading":1.58519, "vx":0.54483, "vy":0.83822, "omega":0.47164, "ax":0.0, "ay":0.0, "alpha":4.54735, "fx":[-21.14127,21.75886,21.14127,-21.75886], "fy":[-21.75886,-21.14127,21.75886,21.14127]},
-    {"t":0.25692, "x":0.83529, "y":0.87849, "heading":1.6025, "vx":0.54483, "vy":0.83822, "omega":0.63854, "ax":0.0, "ay":0.0, "alpha":4.20333, "fx":[-19.1908,20.44798,19.1908,-20.44798], "fy":[-20.44798,-19.1908,20.44798,19.1908]},
-    {"t":0.29362, "x":0.85528, "y":0.90925, "heading":1.62594, "vx":0.54483, "vy":0.83822, "omega":0.79281, "ax":0.0, "ay":0.0, "alpha":3.87644, "fx":[-17.25159,19.26733,17.25159,-19.26733], "fy":[-19.26733,-17.25159,19.26733,17.25159]},
-    {"t":0.33032, "x":0.87528, "y":0.94002, "heading":1.65504, "vx":0.54483, "vy":0.83822, "omega":0.93508, "ax":0.0, "ay":0.0, "alpha":3.56699, "fx":[-15.35189,18.1836,15.35189,-18.18361], "fy":[-18.18361,-15.35189,18.18361,15.35189]},
-    {"t":0.36703, "x":0.89528, "y":0.97078, "heading":1.68936, "vx":0.54483, "vy":0.83822, "omega":1.066, "ax":0.0, "ay":0.0, "alpha":3.27504, "fx":[-13.5142,17.16914,13.5142,-17.16914], "fy":[-17.16914,-13.5142,17.16914,13.5142]},
-    {"t":0.40373, "x":0.91527, "y":1.00155, "heading":1.72848, "vx":0.54483, "vy":0.83822, "omega":1.1862, "ax":0.0, "ay":0.0, "alpha":3.00043, "fx":[-11.75629,16.20174,11.75629,-16.20173], "fy":[-16.20174,-11.75629,16.20173,11.75629]},
-    {"t":0.44043, "x":0.93527, "y":1.03231, "heading":1.77202, "vx":0.54483, "vy":0.83822, "omega":1.29633, "ax":0.0, "ay":0.0, "alpha":2.74279, "fx":[-10.09204,15.26425,10.09204,-15.26425], "fy":[-15.26425,-10.09204,15.26425,10.09204]},
-    {"t":0.47713, "x":0.95527, "y":1.06308, "heading":1.8196, "vx":0.54483, "vy":0.83822, "omega":1.39699, "ax":0.0, "ay":0.0, "alpha":2.50162, "fx":[-8.53208,14.34408,8.53208,-14.34408], "fy":[-14.34408,-8.53208,14.34408,8.53208]},
-    {"t":0.51384, "x":0.97526, "y":1.09384, "heading":1.87087, "vx":0.54483, "vy":0.83822, "omega":1.48881, "ax":0.0, "ay":0.0, "alpha":2.27624, "fx":[-7.0843,13.43254,7.0843,-13.43254], "fy":[-13.43254,-7.0843,13.43254,7.0843]},
-    {"t":0.55054, "x":0.99526, "y":1.12461, "heading":1.92551, "vx":0.54483, "vy":0.83822, "omega":1.57235, "ax":0.0, "ay":0.0, "alpha":2.06591, "fx":[-5.75425,12.52431,5.75426,-12.5243], "fy":[-12.52431,-5.75426,12.5243,5.75425]},
-    {"t":0.58724, "x":1.01526, "y":1.15537, "heading":1.98322, "vx":0.54483, "vy":0.83822, "omega":1.64818, "ax":0.0, "ay":0.0, "alpha":1.86978, "fx":[-4.54549,11.61677,4.5455,-11.61676], "fy":[-11.61677,-4.5455,11.61677,4.54549]},
-    {"t":0.62394, "x":1.03525, "y":1.18614, "heading":2.04371, "vx":0.54483, "vy":0.83822, "omega":1.7168, "ax":0.0, "ay":0.0, "alpha":1.68693, "fx":[-3.45987,10.70953,3.45988,-10.70953], "fy":[-10.70953,-3.45988,10.70953,3.45987]},
-    {"t":0.66065, "x":1.05525, "y":1.2169, "heading":2.10672, "vx":0.54483, "vy":0.83822, "omega":1.77872, "ax":0.0, "ay":0.0, "alpha":1.51644, "fx":[-2.49781,9.8039,2.49781,-9.80389], "fy":[-9.8039,-2.49781,9.80389,2.49781]},
-    {"t":0.69735, "x":1.07525, "y":1.24767, "heading":2.17201, "vx":0.54483, "vy":0.83822, "omega":1.83438, "ax":0.0, "ay":0.0, "alpha":1.35734, "fx":[-1.6585,8.90244,1.65851,-8.90244], "fy":[-8.90244,-1.65851,8.90244,1.6585]},
-    {"t":0.73405, "x":1.09524, "y":1.27843, "heading":2.23933, "vx":0.54483, "vy":0.83822, "omega":1.88419, "ax":0.0, "ay":0.0, "alpha":1.20866, "fx":[-0.94018,8.00867,0.94018,-8.00867], "fy":[-8.00867,-0.94018,8.00867,0.94018]},
-    {"t":0.77075, "x":1.11524, "y":1.3092, "heading":2.30849, "vx":0.54483, "vy":0.83822, "omega":1.92855, "ax":0.0, "ay":0.0, "alpha":1.06943, "fx":[-0.34024,7.12671,0.34024,-7.12671], "fy":[-7.12671,-0.34024,7.12671,0.34024]},
-    {"t":0.80746, "x":1.13524, "y":1.33996, "heading":2.37927, "vx":0.54483, "vy":0.83822, "omega":1.9678, "ax":0.0, "ay":0.0, "alpha":0.93872, "fx":[0.14451,6.26109,-0.14452,-6.2611], "fy":[-6.26109,0.14452,6.2611,-0.14451]},
-    {"t":0.84416, "x":1.15523, "y":1.37073, "heading":2.45149, "vx":0.54483, "vy":0.83822, "omega":2.00226, "ax":0.0, "ay":0.0, "alpha":0.81559, "fx":[0.51777,5.41659,-0.51778,-5.4166], "fy":[-5.41659,0.51778,5.4166,-0.51777]},
-    {"t":0.88086, "x":1.17523, "y":1.40149, "heading":2.52498, "vx":0.54483, "vy":0.83822, "omega":2.03219, "ax":0.0, "ay":0.0, "alpha":0.69914, "fx":[0.78355,4.59807,-0.78356,-4.59809], "fy":[-4.59808,0.78356,4.59809,-0.78355]},
-    {"t":0.91756, "x":1.19523, "y":1.43226, "heading":2.59957, "vx":0.54483, "vy":0.83822, "omega":2.05785, "ax":0.0, "ay":0.0, "alpha":0.58848, "fx":[0.94611,3.81041,-0.94612,-3.81043], "fy":[-3.81042,0.94612,3.81043,-0.94611]},
-    {"t":0.95427, "x":1.21522, "y":1.46302, "heading":2.6751, "vx":0.54483, "vy":0.83822, "omega":2.07945, "ax":0.0, "ay":0.0, "alpha":0.48277, "fx":[1.00981,3.05844,-1.00982,-3.05846], "fy":[-3.05845,1.00982,3.05846,-1.00981]},
-    {"t":0.99097, "x":1.23522, "y":1.49379, "heading":2.75142, "vx":0.54483, "vy":0.83822, "omega":2.09717, "ax":0.0, "ay":0.0, "alpha":0.38116, "fx":[0.97907,2.34691,-0.97908,-2.34692], "fy":[-2.34691,0.97908,2.34692,-0.97907]},
-    {"t":1.02767, "x":1.25522, "y":1.52455, "heading":2.82839, "vx":0.54483, "vy":0.83822, "omega":2.11116, "ax":0.0, "ay":0.0, "alpha":0.28283, "fx":[0.85826,1.68046,-0.85827,-1.68046], "fy":[-1.68046,0.85826,1.68046,-0.85826]},
-    {"t":1.06437, "x":1.27521, "y":1.55532, "heading":2.90587, "vx":0.54483, "vy":0.83822, "omega":2.12154, "ax":0.0, "ay":0.0, "alpha":0.18698, "fx":[0.65167,1.06367,-0.65167,-1.06367], "fy":[-1.06367,0.65167,1.06367,-0.65167]},
-    {"t":1.10108, "x":1.29521, "y":1.58608, "heading":2.98374, "vx":0.54483, "vy":0.83822, "omega":2.1284, "ax":0.0, "ay":0.0, "alpha":0.09278, "fx":[0.36346,0.50108,-0.36346,-0.50107], "fy":[-0.50108,0.36346,0.50107,-0.36346]},
-    {"t":1.13778, "x":1.31521, "y":1.61685, "heading":3.06186, "vx":0.54483, "vy":0.83822, "omega":2.13181, "ax":0.0, "ay":0.0, "alpha":-0.00055, "fx":[-0.00237,-0.00278,0.00238,0.00279], "fy":[0.00278,-0.00238,-0.00279,0.00237]},
-    {"t":1.17448, "x":1.3352, "y":1.64761, "heading":3.1401, "vx":0.54483, "vy":0.83822, "omega":2.13179, "ax":0.0, "ay":0.0, "alpha":-0.09383, "fx":[-0.44197,-0.4433,0.44199,0.44331], "fy":[0.4433,-0.44199,-0.44331,0.44198]},
-    {"t":1.21118, "x":1.3552, "y":1.67838, "heading":-3.06484, "vx":0.54483, "vy":0.83822, "omega":2.12834, "ax":0.0, "ay":0.0, "alpha":-0.18787, "fx":[-0.95164,-0.81573,0.95166,0.81575], "fy":[0.81573,-0.95166,-0.81574,0.95164]},
-    {"t":1.24789, "x":1.3752, "y":1.70914, "heading":-2.98673, "vx":0.54483, "vy":0.83822, "omega":2.12145, "ax":0.0, "ay":0.0, "alpha":-0.28351, "fx":[-1.52773,-1.11514,1.52775,1.11516], "fy":[1.11514,-1.52775,-1.11515,1.52774]},
-    {"t":1.28459, "x":1.39519, "y":1.73991, "heading":-2.90886, "vx":0.54483, "vy":0.83822, "omega":2.11104, "ax":0.0, "ay":0.0, "alpha":-0.38156, "fx":[-2.16662,-1.33634,2.16664,1.33635], "fy":[1.33634,-2.16664,-1.33635,2.16662]},
-    {"t":1.32129, "x":1.41519, "y":1.77067, "heading":-2.83138, "vx":0.54483, "vy":0.83822, "omega":2.09704, "ax":0.0, "ay":0.0, "alpha":-0.48287, "fx":[-2.8646,-1.47387,2.86462,1.47388], "fy":[1.47387,-2.86462,-1.47388,2.86461]},
-    {"t":1.35799, "x":1.43519, "y":1.80144, "heading":-2.75442, "vx":0.54483, "vy":0.83822, "omega":2.07932, "ax":0.0, "ay":0.0, "alpha":-0.58831, "fx":[-3.61781,-1.52201,3.61782,1.52202], "fy":[1.52202,-3.61782,-1.52202,3.61782]},
-    {"t":1.3947, "x":1.45518, "y":1.8322, "heading":-2.6781, "vx":0.54483, "vy":0.83822, "omega":2.05772, "ax":0.0, "ay":0.0, "alpha":-0.69872, "fx":[-4.42213,-1.47481,4.42214,1.47482], "fy":[1.47481,-4.42214,-1.47482,4.42213]},
-    {"t":1.4314, "x":1.47518, "y":1.86296, "heading":-2.60258, "vx":0.54483, "vy":0.83822, "omega":2.03208, "ax":0.0, "ay":0.0, "alpha":-0.815, "fx":[-5.27312,-1.32616,5.27312,1.32615], "fy":[1.32616,-5.27312,-1.32615,5.27312]},
-    {"t":1.4681, "x":1.49518, "y":1.89373, "heading":-2.528, "vx":0.54483, "vy":0.83822, "omega":2.00217, "ax":0.0, "ay":0.0, "alpha":-0.93802, "fx":[-6.16599,-1.06987,6.16598,1.06987], "fy":[1.06987,-6.16598,-1.06987,6.16599]},
-    {"t":1.5048, "x":1.51517, "y":1.92449, "heading":-2.45451, "vx":0.54483, "vy":0.83822, "omega":1.96774, "ax":0.0, "ay":0.0, "alpha":-1.06871, "fx":[-7.09556,-0.69987,7.09555,0.69987], "fy":[0.69987,-7.09555,-0.69987,7.09556]},
-    {"t":1.54151, "x":1.53517, "y":1.95526, "heading":-2.38229, "vx":0.54483, "vy":0.83822, "omega":1.92851, "ax":0.0, "ay":0.0, "alpha":-1.20797, "fx":[-8.05635,-0.21029,8.05634,0.21028], "fy":[0.21029,-8.05634,-0.21028,8.05635]},
-    {"t":1.57821, "x":1.55517, "y":1.98602, "heading":-2.31151, "vx":0.54483, "vy":0.83822, "omega":1.88418, "ax":0.0, "ay":0.0, "alpha":-1.35674, "fx":[-9.04261,0.40434,9.0426,-0.40435], "fy":[-0.40434,-9.04261,0.40434,9.04261]},
-    {"t":1.61491, "x":1.57516, "y":2.01679, "heading":-2.24235, "vx":0.54483, "vy":0.83822, "omega":1.83438, "ax":0.0, "ay":0.0, "alpha":-1.51597, "fx":[-10.04849,1.14888,10.04848,-1.14888], "fy":[-1.14888,-10.04848,1.14888,10.04849]},
-    {"t":1.65161, "x":1.59516, "y":2.04755, "heading":-2.17503, "vx":0.54483, "vy":0.83822, "omega":1.77874, "ax":0.0, "ay":0.0, "alpha":-1.6866, "fx":[-11.06818,2.0274,11.06817,-2.02741], "fy":[-2.0274,-11.06817,2.02741,11.06818]},
-    {"t":1.68832, "x":1.61516, "y":2.07832, "heading":-2.10974, "vx":0.54483, "vy":0.83822, "omega":1.71684, "ax":0.0, "ay":0.0, "alpha":-1.86958, "fx":[-12.09621,3.04297,12.09621,-3.04297], "fy":[-3.04297,-12.09621,3.04297,12.09621]},
-    {"t":1.72502, "x":1.63515, "y":2.10908, "heading":-2.04673, "vx":0.54483, "vy":0.83822, "omega":1.64822, "ax":0.0, "ay":0.0, "alpha":-2.06583, "fx":[-13.12772,4.19739,13.12772,-4.19739], "fy":[-4.19739,-13.12772,4.19739,13.12772]},
-    {"t":1.76172, "x":1.65515, "y":2.13985, "heading":-1.98624, "vx":0.54483, "vy":0.83822, "omega":1.5724, "ax":0.0, "ay":0.0, "alpha":-2.27626, "fx":[-14.15883,5.49098,14.15883,-5.49098], "fy":[-5.49098,-14.15883,5.49098,14.15883]},
-    {"t":1.79842, "x":1.67515, "y":2.17061, "heading":-1.92853, "vx":0.54483, "vy":0.83822, "omega":1.48886, "ax":0.0, "ay":0.0, "alpha":-2.5017, "fx":[-15.18711,6.9223,15.18712,-6.9223], "fy":[-6.9223,-15.18711,6.9223,15.18711]},
-    {"t":1.83513, "x":1.69514, "y":2.20138, "heading":-1.87388, "vx":0.54483, "vy":0.83822, "omega":1.39704, "ax":0.0, "ay":0.0, "alpha":-2.74291, "fx":[-16.21206,8.48788,16.21207,-8.48788], "fy":[-8.48788,-16.21206,8.48788,16.21206]},
-    {"t":1.87183, "x":1.71514, "y":2.23214, "heading":-1.82261, "vx":0.54483, "vy":0.83822, "omega":1.29637, "ax":0.0, "ay":0.0, "alpha":-3.00056, "fx":[-17.23572,10.18193,17.23572,-10.18192], "fy":[-10.18193,-17.23572,10.18192,17.23572]},
-    {"t":1.90853, "x":1.73514, "y":2.26291, "heading":-1.77503, "vx":0.54483, "vy":0.83822, "omega":1.18624, "ax":0.0, "ay":0.0, "alpha":-3.27517, "fx":[-18.26325,11.99599,18.26325,-11.99599], "fy":[-11.99599,-18.26325,11.99599,18.26325]},
-    {"t":1.94523, "x":1.75513, "y":2.29367, "heading":-1.73149, "vx":0.54483, "vy":0.83822, "omega":1.06603, "ax":0.0, "ay":0.0, "alpha":-3.56711, "fx":[-19.30365,13.91863,19.30365,-13.91863], "fy":[-13.91863,-19.30365,13.91863,19.30365]},
-    {"t":1.98194, "x":1.77513, "y":2.32444, "heading":-1.69236, "vx":0.54483, "vy":0.83822, "omega":0.93511, "ax":0.0, "ay":0.0, "alpha":-3.87653, "fx":[-20.3704,15.93499,20.37041,-15.93499], "fy":[-15.93499,-20.3704,15.93499,20.3704]},
-    {"t":2.01864, "x":1.79513, "y":2.3552, "heading":-1.65804, "vx":0.54483, "vy":0.83822, "omega":0.79283, "ax":0.0, "ay":0.0, "alpha":-4.20339, "fx":[-21.48208,18.02635,21.48208,-18.02635], "fy":[-18.02635,-21.48208,18.02635,21.48208]},
-    {"t":2.05534, "x":1.81512, "y":2.38597, "heading":-1.62894, "vx":0.54483, "vy":0.83822, "omega":0.63856, "ax":0.0, "ay":0.0, "alpha":-4.54738, "fx":[-22.66288,20.16948,22.66288,-20.16949], "fy":[-20.16948,-22.66288,20.16949,22.66288]},
-    {"t":2.09204, "x":1.83512, "y":2.41673, "heading":-1.60551, "vx":0.54483, "vy":0.83822, "omega":0.47166, "ax":0.0, "ay":0.0, "alpha":-4.90795, "fx":[-23.94303,22.33592,23.94293,-22.33601], "fy":[-22.33593,-23.94295,22.336,23.943]},
-    {"t":2.12875, "x":1.85512, "y":2.4475, "heading":-1.5882, "vx":0.54483, "vy":0.83822, "omega":0.29152, "ax":-0.00013, "ay":0.00001, "alpha":-5.28426, "fx":[-25.36075,24.48905,25.35649,-24.49329], "fy":[-24.49088,-25.35866,24.49148,25.35858]},
-    {"t":2.16545, "x":1.87511, "y":2.47826, "heading":-1.5775, "vx":0.54483, "vy":0.83822, "omega":0.09758, "ax":-2.55761, "ay":-3.93519, "alpha":-2.6077, "fx":[-51.72521,-24.69926,-31.26056,-61.45938], "fy":[-71.33006,-79.3859,-59.60579,-49.92723]},
-    {"t":2.20215, "x":1.89339, "y":2.50638, "heading":-1.57391, "vx":0.45096, "vy":0.69379, "omega":0.00187, "ax":-4.08943, "ay":-6.29151, "alpha":-0.02639, "fx":[-67.67499,-67.32506,-67.54967,-67.89981], "fy":[-103.98145,-104.20712,-104.05972,-103.83285]},
-    {"t":2.23885, "x":1.90719, "y":2.5276, "heading":-1.57385, "vx":0.30086, "vy":0.46288, "omega":0.0009, "ax":-4.09736, "ay":-6.30374, "alpha":-0.01428, "fx":[-67.77723,-67.58773,-67.70973,-67.89928], "fy":[-104.20107,-104.32374,-104.24407,-104.12105]},
-    {"t":2.27556, "x":1.91547, "y":2.54035, "heading":-1.57381, "vx":0.15048, "vy":0.23151, "omega":0.00037, "ax":-4.10002, "ay":-6.30784, "alpha":-0.01021, "fx":[-67.81157,-67.67592,-67.76334,-67.89902], "fy":[-104.27488,-104.36281,-104.30581,-104.2177]},
-    {"t":2.31226, "x":1.91823, "y":2.54459, "heading":-1.5738, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.48298, "ay":6.04247, "alpha":0.00748, "fx":[74.10727,74.19831,74.1309,74.03985], "fy":[99.91144,99.84398,99.89421,99.96158]},
+    {"t":0.03804, "x":0.43876, "y":0.55047, "heading":1.5708, "vx":0.17053, "vy":0.22986, "omega":0.00028, "ax":4.48025, "ay":6.03879, "alpha":0.01044, "fx":[74.05746,74.18442,74.09049,73.96351], "fy":[99.85392,99.7599,99.83004,99.92389]},
+    {"t":0.07608, "x":0.44849, "y":0.56358, "heading":1.57081, "vx":0.34097, "vy":0.45958, "omega":0.00068, "ax":4.4721, "ay":6.02779, "alpha":0.01929, "fx":[73.90843,74.14274,73.96989,73.73553], "fy":[99.68184,99.50867,99.63855,99.81113]},
+    {"t":0.11412, "x":0.4647, "y":0.58542, "heading":1.57083, "vx":0.51109, "vy":0.68888, "omega":0.00142, "ax":2.22369, "ay":2.99735, "alpha":2.81362, "fx":[24.77598,54.67068,47.33322,20.2811], "fy":[40.1773,34.31597,58.72416,65.00891]},
+    {"t":0.15216, "x":0.48575, "y":0.6138, "heading":1.57089, "vx":0.59568, "vy":0.8029, "omega":0.10845, "ax":0.00005, "ay":0.0, "alpha":4.22282, "fx":[-19.91862,19.92402,19.9204,-19.92222], "fy":[-19.9231,-19.9194,19.92314,19.91962]},
+    {"t":0.1902, "x":0.50841, "y":0.64434, "heading":1.57501, "vx":0.59568, "vy":0.8029, "omega":0.26908, "ax":0.0, "ay":0.0, "alpha":3.89866, "fx":[-18.31433,18.46944,18.31436,-18.46941], "fy":[-18.46944,-18.31435,18.46941,18.31433]},
+    {"t":0.22824, "x":0.53107, "y":0.67488, "heading":1.58525, "vx":0.59568, "vy":0.8029, "omega":0.41739, "ax":0.0, "ay":0.0, "alpha":3.59311, "fx":[-16.70387,17.1938,16.70387,-17.1938], "fy":[-17.1938,-16.70387,17.1938,16.70387]},
+    {"t":0.26628, "x":0.55373, "y":0.70542, "heading":1.60113, "vx":0.59568, "vy":0.8029, "omega":0.55407, "ax":0.0, "ay":0.0, "alpha":3.30606, "fx":[-15.11632,16.06225,15.11632,-16.06225], "fy":[-16.06225,-15.11632,16.06225,15.11632]},
+    {"t":0.30432, "x":0.57639, "y":0.73597, "heading":1.6222, "vx":0.59568, "vy":0.8029, "omega":0.67984, "ax":0.0, "ay":0.0, "alpha":3.03722, "fx":[-13.57301,15.04551,13.57301,-15.04551], "fy":[-15.04551,-13.57301,15.04551,13.57301]},
+    {"t":0.34236, "x":0.59905, "y":0.76651, "heading":1.64806, "vx":0.59568, "vy":0.8029, "omega":0.79538, "ax":0.0, "ay":0.0, "alpha":2.78611, "fx":[-12.08977,14.11892,12.08977,-14.11892], "fy":[-14.11892,-12.08977,14.11892,12.08977]},
+    {"t":0.3804, "x":0.62171, "y":0.79705, "heading":1.67832, "vx":0.59568, "vy":0.8029, "omega":0.90136, "ax":0.0, "ay":0.0, "alpha":2.55213, "fx":[-10.67813,13.26229,10.67813,-13.26229], "fy":[-13.26229,-10.67813,13.26229,10.67813]},
+    {"t":0.41844, "x":0.64437, "y":0.82759, "heading":1.71261, "vx":0.59568, "vy":0.8029, "omega":0.99844, "ax":0.0, "ay":0.0, "alpha":2.33456, "fx":[-9.3462,12.45943,9.3462,-12.45943], "fy":[-12.45943,-9.3462,12.45943,9.3462]},
+    {"t":0.45649, "x":0.66703, "y":0.85814, "heading":1.75059, "vx":0.59568, "vy":0.8029, "omega":1.08725, "ax":0.0, "ay":0.0, "alpha":2.13262, "fx":[-8.09942,11.69769,8.09942,-11.69769], "fy":[-11.69769,-8.09942,11.69769,8.09942]},
+    {"t":0.49453, "x":0.68969, "y":0.88868, "heading":1.79195, "vx":0.59568, "vy":0.8029, "omega":1.16838, "ax":0.0, "ay":0.0, "alpha":1.94547, "fx":[-6.94109,10.96751,6.94109,-10.96751], "fy":[-10.96751,-6.94109,10.96751,6.94109]},
+    {"t":0.53257, "x":0.71235, "y":0.91922, "heading":1.8364, "vx":0.59568, "vy":0.8029, "omega":1.24239, "ax":0.0, "ay":0.0, "alpha":1.77223, "fx":[-5.87284,10.26192,5.87284,-10.26192], "fy":[-10.26192,-5.87284,10.26192,5.87284]},
+    {"t":0.57061, "x":0.73501, "y":0.94976, "heading":1.88366, "vx":0.59568, "vy":0.8029, "omega":1.3098, "ax":0.0, "ay":0.0, "alpha":1.612, "fx":[-4.89493,9.57607,4.89493,-9.57607], "fy":[-9.57607,-4.89493,9.57607,4.89493]},
+    {"t":0.60865, "x":0.75767, "y":0.98031, "heading":1.93348, "vx":0.59568, "vy":0.8029, "omega":1.37112, "ax":0.0, "ay":0.0, "alpha":1.46389, "fx":[-4.00655,8.90682,4.00655,-8.90682], "fy":[-8.90682,-4.00655,8.90682,4.00655]},
+    {"t":0.64669, "x":0.78033, "y":1.01085, "heading":1.98564, "vx":0.59568, "vy":0.8029, "omega":1.42681, "ax":0.0, "ay":0.0, "alpha":1.32701, "fx":[-3.20606,8.2524,3.20606,-8.25239], "fy":[-8.2524,-3.20606,8.25239,3.20606]},
+    {"t":0.68473, "x":0.80299, "y":1.04139, "heading":2.03992, "vx":0.59568, "vy":0.8029, "omega":1.47729, "ax":0.0, "ay":0.0, "alpha":1.20051, "fx":[-2.49114,7.61203,2.49114,-7.61203], "fy":[-7.61203,-2.49114,7.61203,2.49114]},
+    {"t":0.72277, "x":0.82565, "y":1.07193, "heading":2.09611, "vx":0.59568, "vy":0.8029, "omega":1.52296, "ax":0.0, "ay":0.0, "alpha":1.08353, "fx":[-1.85897,6.98575,1.85897,-6.98575], "fy":[-6.98575,-1.85897,6.98575,1.85897]},
+    {"t":0.76081, "x":0.84831, "y":1.10248, "heading":2.15405, "vx":0.59568, "vy":0.8029, "omega":1.56418, "ax":0.0, "ay":0.0, "alpha":0.97527, "fx":[-1.30635,6.37411,1.30635,-6.37411], "fy":[-6.37411,-1.30635,6.37411,1.30635]},
+    {"t":0.79885, "x":0.87097, "y":1.13302, "heading":2.21355, "vx":0.59568, "vy":0.8029, "omega":1.60127, "ax":0.0, "ay":0.0, "alpha":0.87495, "fx":[-0.82984,5.77802,0.82984,-5.77802], "fy":[-5.77802,-0.82984,5.77802,0.82984]},
+    {"t":0.83689, "x":0.89363, "y":1.16356, "heading":2.27446, "vx":0.59568, "vy":0.8029, "omega":1.63456, "ax":0.0, "ay":0.0, "alpha":0.78183, "fx":[-0.42584,5.19863,0.42584,-5.19863], "fy":[-5.19863,-0.42584,5.19863,0.42584]},
+    {"t":0.87493, "x":0.91629, "y":1.1941, "heading":2.33664, "vx":0.59568, "vy":0.8029, "omega":1.6643, "ax":0.0, "ay":0.0, "alpha":0.6952, "fx":[-0.09068,4.6372,0.09068,-4.63721], "fy":[-4.6372,-0.09068,4.63721,0.09068]},
+    {"t":0.91297, "x":0.93895, "y":1.22465, "heading":2.39995, "vx":0.59568, "vy":0.8029, "omega":1.69074, "ax":0.0, "ay":0.0, "alpha":0.61439, "fx":[0.1793,4.09506,-0.17931,-4.09507], "fy":[-4.09506,0.17931,4.09507,-0.1793]},
+    {"t":0.95101, "x":0.96161, "y":1.25519, "heading":2.46427, "vx":0.59568, "vy":0.8029, "omega":1.71412, "ax":0.0, "ay":0.0, "alpha":0.53877, "fx":[0.38771,3.57351,-0.38772,-3.57351], "fy":[-3.57351,0.38772,3.57351,-0.38771]},
+    {"t":0.98905, "x":0.98427, "y":1.28573, "heading":2.52947, "vx":0.59568, "vy":0.8029, "omega":1.73461, "ax":0.0, "ay":0.0, "alpha":0.46773, "fx":[0.53802,3.0738,-0.53803,-3.0738], "fy":[-3.0738,0.53803,3.0738,-0.53802]},
+    {"t":1.02709, "x":1.00693, "y":1.31627, "heading":2.59546, "vx":0.59568, "vy":0.8029, "omega":1.7524, "ax":0.0, "ay":0.0, "alpha":0.4007, "fx":[0.63354,2.59714,-0.63355,-2.59715], "fy":[-2.59715,0.63355,2.59715,-0.63354]},
+    {"t":1.06513, "x":1.02959, "y":1.34682, "heading":2.66212, "vx":0.59568, "vy":0.8029, "omega":1.76765, "ax":0.0, "ay":0.0, "alpha":0.33712, "fx":[0.67738,2.14469,-0.67739,-2.1447], "fy":[-2.14469,0.67739,2.1447,-0.67739]},
+    {"t":1.10317, "x":1.05225, "y":1.37736, "heading":2.72936, "vx":0.59568, "vy":0.8029, "omega":1.78047, "ax":0.0, "ay":0.0, "alpha":0.27647, "fx":[0.67244,1.71753,-0.67244,-1.71753], "fy":[-1.71753,0.67244,1.71753,-0.67244]},
+    {"t":1.14121, "x":1.07491, "y":1.4079, "heading":2.79709, "vx":0.59568, "vy":0.8029, "omega":1.79099, "ax":0.0, "ay":0.0, "alpha":0.21823, "fx":[0.62133,1.31672,-0.62134,-1.31672], "fy":[-1.31672,0.62134,1.31672,-0.62133]},
+    {"t":1.17925, "x":1.09757, "y":1.43844, "heading":2.86522, "vx":0.59568, "vy":0.8029, "omega":1.79929, "ax":0.0, "ay":0.0, "alpha":0.16192, "fx":[0.52645,0.94332,-0.52645,-0.94332], "fy":[-0.94332,0.52645,0.94332,-0.52645]},
+    {"t":1.21729, "x":1.12023, "y":1.46899, "heading":2.93367, "vx":0.59568, "vy":0.8029, "omega":1.80545, "ax":0.0, "ay":0.0, "alpha":0.10705, "fx":[0.38989,0.59839,-0.38989,-0.59839], "fy":[-0.59839,0.38989,0.59839,-0.38989]},
+    {"t":1.25533, "x":1.14288, "y":1.49953, "heading":3.00235, "vx":0.59568, "vy":0.8029, "omega":1.80952, "ax":0.0, "ay":0.0, "alpha":0.05314, "fx":[0.21348,0.28308,-0.21348,-0.28307], "fy":[-0.28308,0.21348,0.28307,-0.21348]},
+    {"t":1.29337, "x":1.16554, "y":1.53007, "heading":3.07119, "vx":0.59568, "vy":0.8029, "omega":1.81154, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[-0.00119,-0.00137,0.0012,0.00138], "fy":[0.00137,-0.0012,-0.00138,0.00119]},
+    {"t":1.33142, "x":1.1882, "y":1.56061, "heading":3.1401, "vx":0.59568, "vy":0.8029, "omega":1.81153, "ax":0.0, "ay":0.0, "alpha":-0.05367, "fx":[-0.25281,-0.25356,0.25281,0.25357], "fy":[0.25356,-0.25281,-0.25357,0.25281]},
+    {"t":1.36946, "x":1.21086, "y":1.59116, "heading":-3.07418, "vx":0.59568, "vy":0.8029, "omega":1.80949, "ax":0.0, "ay":0.0, "alpha":-0.10752, "fx":[-0.54024,-0.4719,0.54025,0.47191], "fy":[0.4719,-0.54025,-0.47191,0.54024]},
+    {"t":1.4075, "x":1.23352, "y":1.6217, "heading":-3.00534, "vx":0.59568, "vy":0.8029, "omega":1.8054, "ax":0.0, "ay":0.0, "alpha":-0.1623, "fx":[-0.86256,-0.65456,0.86256,0.65457], "fy":[0.65456,-0.86256,-0.65457,0.86256]},
+    {"t":1.44554, "x":1.25618, "y":1.65224, "heading":-2.93666, "vx":0.59568, "vy":0.8029, "omega":1.79923, "ax":0.0, "ay":0.0, "alpha":-0.2185, "fx":[-1.21895,-0.79944,1.21896,0.79944], "fy":[0.79944,-1.21896,-0.79944,1.21895]},
+    {"t":1.48358, "x":1.27884, "y":1.68279, "heading":-2.86822, "vx":0.59568, "vy":0.8029, "omega":1.79092, "ax":0.0, "ay":0.0, "alpha":-0.2766, "fx":[-1.60872,-0.90413,1.60872,0.90414], "fy":[0.90414,-1.60872,-0.90414,1.60872]},
+    {"t":1.52162, "x":1.3015, "y":1.71333, "heading":-2.80009, "vx":0.59568, "vy":0.8029, "omega":1.78039, "ax":0.0, "ay":0.0, "alpha":-0.33712, "fx":[-2.03118,-0.96593,2.03118,0.96594], "fy":[0.96593,-2.03118,-0.96594,2.03118]},
+    {"t":1.55966, "x":1.32416, "y":1.74387, "heading":-2.73237, "vx":0.59568, "vy":0.8029, "omega":1.76757, "ax":0.0, "ay":0.0, "alpha":-0.40058, "fx":[-2.48565,-0.98178,2.48565,0.98179], "fy":[0.98179,-2.48565,-0.98179,2.48565]},
+    {"t":1.5977, "x":1.34682, "y":1.77441, "heading":-2.66513, "vx":0.59568, "vy":0.8029, "omega":1.75233, "ax":0.0, "ay":0.0, "alpha":-0.46751, "fx":[-2.97138,-0.94832,2.97138,0.94832], "fy":[0.94832,-2.97138,-0.94832,2.97138]},
+    {"t":1.63574, "x":1.36948, "y":1.80496, "heading":-2.59847, "vx":0.59568, "vy":0.8029, "omega":1.73455, "ax":0.0, "ay":0.0, "alpha":-0.53847, "fx":[-3.48754,-0.86187,3.48754,0.86187], "fy":[0.86187,-3.48754,-0.86187,3.48754]},
+    {"t":1.67378, "x":1.39214, "y":1.8355, "heading":-2.53249, "vx":0.59568, "vy":0.8029, "omega":1.71406, "ax":0.0, "ay":0.0, "alpha":-0.61404, "fx":[-4.03313,-0.71846,4.03312,0.71846], "fy":[0.71846,-4.03313,-0.71846,4.03313]},
+    {"t":1.71182, "x":1.4148, "y":1.86604, "heading":-2.46728, "vx":0.59568, "vy":0.8029, "omega":1.6907, "ax":0.0, "ay":0.0, "alpha":-0.69482, "fx":[-4.60701,-0.5139,4.60701,0.51389], "fy":[0.5139,-4.60701,-0.5139,4.60701]},
+    {"t":1.74986, "x":1.43746, "y":1.89658, "heading":-2.40297, "vx":0.59568, "vy":0.8029, "omega":1.66427, "ax":0.0, "ay":0.0, "alpha":-0.78146, "fx":[-5.20788,-0.24376,5.20788,0.24376], "fy":[0.24376,-5.20788,-0.24376,5.20788]},
+    {"t":1.7879, "x":1.46012, "y":1.92713, "heading":-2.33966, "vx":0.59568, "vy":0.8029, "omega":1.63455, "ax":0.0, "ay":0.0, "alpha":-0.87461, "fx":[-5.83426,0.09649,5.83426,-0.09649], "fy":[-0.09649,-5.83426,0.09649,5.83426]},
+    {"t":1.82594, "x":1.48278, "y":1.95767, "heading":-2.27748, "vx":0.59568, "vy":0.8029, "omega":1.60128, "ax":0.0, "ay":0.0, "alpha":-0.97498, "fx":[-6.48454,0.51149,6.48454,-0.5115], "fy":[-0.51149,-6.48454,0.5115,6.48454]},
+    {"t":1.86398, "x":1.50544, "y":1.98821, "heading":-2.21656, "vx":0.59568, "vy":0.8029, "omega":1.56419, "ax":0.0, "ay":0.0, "alpha":-1.0833, "fx":[-7.15703,1.00588,7.15703,-1.00588], "fy":[-1.00588,-7.15703,1.00588,7.15703]},
+    {"t":1.90202, "x":1.5281, "y":2.01875, "heading":-2.15706, "vx":0.59568, "vy":0.8029, "omega":1.52298, "ax":0.0, "ay":0.0, "alpha":-1.20035, "fx":[-7.84999,1.58418,7.84999,-1.58418], "fy":[-1.58418,-7.84999,1.58418,7.84999]},
+    {"t":1.94006, "x":1.55076, "y":2.0493, "heading":-2.09913, "vx":0.59568, "vy":0.8029, "omega":1.47732, "ax":0.0, "ay":0.0, "alpha":-1.32692, "fx":[-8.56177,2.25074,8.56177,-2.25074], "fy":[-2.25074,-8.56177,2.25074,8.56177]},
+    {"t":1.9781, "x":1.57342, "y":2.07984, "heading":-2.04293, "vx":0.59568, "vy":0.8029, "omega":1.42684, "ax":0.0, "ay":0.0, "alpha":-1.46385, "fx":[-9.29094,3.00962,9.29094,-3.00962], "fy":[-3.00962,-9.29094,3.00962,9.29094]},
+    {"t":2.01614, "x":1.59608, "y":2.11038, "heading":-1.98865, "vx":0.59568, "vy":0.8029, "omega":1.37115, "ax":0.0, "ay":0.0, "alpha":-1.61201, "fx":[-10.03641,3.8644,10.03642,-3.8644], "fy":[-3.8644,-10.03642,3.8644,10.03642]},
+    {"t":2.05418, "x":1.61874, "y":2.14092, "heading":-1.93649, "vx":0.59568, "vy":0.8029, "omega":1.30983, "ax":0.0, "ay":0.0, "alpha":-1.77227, "fx":[-10.79772,4.8181,10.79772,-4.8181], "fy":[-4.8181,-10.79772,4.8181,10.79772]},
+    {"t":2.09222, "x":1.6414, "y":2.17147, "heading":-1.88667, "vx":0.59568, "vy":0.8029, "omega":1.24241, "ax":0.0, "ay":0.0, "alpha":-1.94554, "fx":[-11.57523,5.87294,11.57523,-5.87294], "fy":[-5.87294,-11.57523,5.87294,11.57523]},
+    {"t":2.13026, "x":1.66406, "y":2.20201, "heading":-1.8394, "vx":0.59568, "vy":0.8029, "omega":1.1684, "ax":0.0, "ay":0.0, "alpha":-2.13271, "fx":[-12.37046,7.03022,12.37046,-7.03021], "fy":[-7.03022,-12.37046,7.03021,12.37046]},
+    {"t":2.1683, "x":1.68672, "y":2.23255, "heading":-1.79496, "vx":0.59568, "vy":0.8029, "omega":1.08728, "ax":0.0, "ay":0.0, "alpha":-2.33465, "fx":[-13.18646,8.28997,13.18646,-8.28997], "fy":[-8.28997,-13.18646,8.28997,13.18646]},
+    {"t":2.20635, "x":1.70938, "y":2.26309, "heading":-1.7536, "vx":0.59568, "vy":0.8029, "omega":0.99846, "ax":0.0, "ay":0.0, "alpha":-2.55221, "fx":[-14.02822,9.6508,14.02822,-9.6508], "fy":[-9.6508,-14.02822,9.6508,14.02822]},
+    {"t":2.24439, "x":1.73204, "y":2.29364, "heading":-1.71562, "vx":0.59568, "vy":0.8029, "omega":0.90138, "ax":0.0, "ay":0.0, "alpha":-2.78618, "fx":[-14.90314,11.10946,14.90314,-11.10946], "fy":[-11.10946,-14.90314,11.10946,14.90314]},
+    {"t":2.28243, "x":1.7547, "y":2.32418, "heading":-1.68133, "vx":0.59568, "vy":0.8029, "omega":0.79539, "ax":0.0, "ay":0.0, "alpha":-3.03727, "fx":[-15.82152,12.6605,15.82152,-12.6605], "fy":[-12.6605,-15.82152,12.6605,15.82152]},
+    {"t":2.32047, "x":1.77736, "y":2.35472, "heading":-1.65107, "vx":0.59568, "vy":0.8029, "omega":0.67985, "ax":0.0, "ay":0.0, "alpha":-3.3061, "fx":[-16.79708,14.29577,16.79708,-14.29577], "fy":[-14.29577,-16.79708,14.29577,16.79708]},
+    {"t":2.35851, "x":1.80002, "y":2.38526, "heading":-1.62521, "vx":0.59568, "vy":0.8029, "omega":0.55409, "ax":0.0, "ay":0.0, "alpha":-3.59313, "fx":[-17.84749,16.00377,17.84749,-16.00377], "fy":[-16.00377,-17.84749,16.00377,17.84749]},
+    {"t":2.39655, "x":1.82268, "y":2.41581, "heading":-1.60413, "vx":0.59568, "vy":0.8029, "omega":0.4174, "ax":0.0, "ay":0.0, "alpha":-3.89867, "fx":[-18.99486,17.76889,18.99483,-17.76892], "fy":[-17.76889,-18.99483,17.76892,18.99485]},
+    {"t":2.43459, "x":1.84534, "y":2.44635, "heading":-1.58825, "vx":0.59568, "vy":0.8029, "omega":0.26909, "ax":-0.00006, "ay":0.0, "alpha":-4.22282, "fx":[-20.2669,19.56961,20.26507,-19.57145], "fy":[-19.57053,-20.26608,19.57053,20.26589]},
+    {"t":2.47263, "x":1.868, "y":2.47689, "heading":-1.57802, "vx":0.59568, "vy":0.8029, "omega":0.10846, "ax":-2.22369, "ay":-2.99736, "alpha":-2.81387, "fx":[-47.44425,-20.37853,-24.62861,-54.60944], "fy":[-58.62488,-65.06062,-40.30922,-34.23173]},
+    {"t":2.51067, "x":1.88905, "y":2.50526, "heading":-1.57389, "vx":0.51109, "vy":0.68888, "omega":0.00142, "ax":-4.4721, "ay":-6.02779, "alpha":-0.01929, "fx":[-73.97053,-73.73563,-73.90781,-74.14266], "fy":[-99.63807,-99.81106,-99.6823,-99.50873]},
+    {"t":2.54871, "x":1.90526, "y":2.52711, "heading":-1.57384, "vx":0.34097, "vy":0.45958, "omega":0.00068, "ax":-4.48025, "ay":-6.03879, "alpha":-0.01044, "fx":[-74.09083,-73.96357,-74.05712,-74.18437], "fy":[-99.82979,-99.92385,-99.85417,-99.75993]},
+    {"t":2.58675, "x":1.91499, "y":2.54022, "heading":-1.57381, "vx":0.17053, "vy":0.22986, "omega":0.00028, "ax":-4.48298, "ay":-6.04247, "alpha":-0.00748, "fx":[-74.13114,-74.03989,-74.10703,-74.19828], "fy":[-99.89403,-99.96156,-99.91161,-99.844]},
+    {"t":2.62479, "x":1.91823, "y":2.54459, "heading":-1.5738, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/OtoRR.traj b/src/main/deploy/choreo/OtoRR.traj
index 13591b5f..d07a379c 100644
--- a/src/main/deploy/choreo/OtoRR.traj
+++ b/src/main/deploy/choreo/OtoRR.traj
@@ -3,7 +3,7 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":1.6156142950057983, "y":0.7216874957084656, "heading":0.0, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":2.5106818675994873, "y":0.6606601476669312, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
@@ -15,7 +15,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.6156142950057983 m", "val":1.6156142950057983}, "y":{"exp":"0.7216874957084656 m", "val":0.7216874957084656}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"RR.x", "val":2.5106818675994873}, "y":{"exp":"RR.y", "val":0.6606601476669312}, "heading":{"exp":"RR.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
@@ -53,49 +53,51 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,0.60877,0.87919],
+  "waypoints":[0.0,0.69,0.94864],
   "samples":[
-    {"t":0.0, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.41268, "ay":0.09757, "alpha":-15.33447, "fx":[109.77909,1.37507,1.58789,112.95133], "fy":[58.49673,124.2093,-124.15556,-52.09794]},
-    {"t":0.02341, "x":0.73312, "y":0.71989, "heading":1.5708, "vx":0.07991, "vy":0.00228, "omega":-0.35905, "ax":3.48786, "ay":0.09838, "alpha":-15.19731, "fx":[109.97486,3.5037,4.05089,113.13558], "fy":[58.10652,124.14413,-124.07212,-51.672]},
-    {"t":0.04683, "x":0.73594, "y":0.71997, "heading":1.56239, "vx":0.16157, "vy":0.00459, "omega":-0.71488, "ax":3.57327, "ay":0.0815, "alpha":-15.0383, "fx":[110.47891,6.8929,5.85183,113.09032], "fy":[57.11827,123.97836,-123.96436,-51.74222]},
-    {"t":0.07024, "x":0.74071, "y":0.7201, "heading":1.54565, "vx":0.24524, "vy":0.0065, "omega":-1.06699, "ax":3.67128, "ay":0.04577, "alpha":-14.85083, "fx":[111.26965,11.5183,7.17553,112.83204], "fy":[55.5343,123.61015,-123.84795,-52.26946]},
-    {"t":0.09366, "x":0.74745, "y":0.72026, "heading":1.52067, "vx":0.3312, "vy":0.00757, "omega":-1.41471, "ax":3.78468, "ay":-0.00995, "alpha":-14.62568, "fx":[112.3151,17.33231,8.26999,112.37756], "fy":[53.35534,122.90467,-123.71795,-53.20022]},
-    {"t":0.11707, "x":0.75625, "y":0.72044, "heading":1.48754, "vx":0.41981, "vy":0.00734, "omega":-1.75716, "ax":3.91773, "ay":-0.0866, "alpha":-14.34834, "fx":[113.57313,24.26783,9.50438,111.74917], "fy":[50.58204,121.69871,-123.54643,-54.46151]},
-    {"t":0.14049, "x":0.76715, "y":0.72059, "heading":1.4464, "vx":0.51154, "vy":0.00531, "omega":-2.09312, "ax":4.07805, "ay":-0.18443, "alpha":-13.99391, "fx":[114.9919,32.24479,11.47816,110.98222], "fy":[47.21717,119.8033,-123.26475,-55.95267]},
-    {"t":0.1639, "x":0.78024, "y":0.72066, "heading":1.39739, "vx":0.60703, "vy":0.00099, "omega":-2.42078, "ax":4.28017, "ay":-0.30176, "alpha":-13.51661, "fx":[116.51021,41.17849,15.23809,110.13705], "fy":[43.26872,117.00067,-122.69457,-57.53163]},
-    {"t":0.18731, "x":0.79563, "y":0.7206, "heading":1.34071, "vx":0.70725, "vy":-0.00608, "omega":-2.73726, "ax":4.55255, "ay":-0.43092, "alpha":-12.82758, "fx":[118.05818,50.98825,22.71335,109.31764], "fy":[38.75475,113.03122,-121.29362,-58.99086]},
-    {"t":0.21073, "x":0.81344, "y":0.72034, "heading":1.27662, "vx":0.81384, "vy":-0.01617, "omega":-3.03761, "ax":4.94879, "ay":-0.54382, "alpha":-11.75141, "fx":[119.55808,61.6026,37.42044,108.70112], "fy":[33.71104,107.56452,-117.227,-60.0131]},
-    {"t":0.23414, "x":0.83385, "y":0.71981, "heading":1.2055, "vx":0.92971, "vy":-0.0289, "omega":-3.31276, "ax":5.54025, "ay":-0.54949, "alpha":-10.00625, "fx":[120.92582,72.95272,63.93718,108.58177], "fy":[28.2049,100.14593,-104.60381,-60.08654]},
-    {"t":0.25756, "x":0.85714, "y":0.71899, "heading":1.12793, "vx":1.05944, "vy":-0.04177, "omega":-3.54705, "ax":6.26621, "ay":-0.28789, "alpha":-7.60035, "fx":[122.07405,84.94315,97.96419,109.4266], "fy":[22.35965,90.09988,-73.16702,-58.33149]},
-    {"t":0.28097, "x":0.88366, "y":0.71793, "heading":1.04488, "vx":1.20615, "vy":-0.04851, "omega":-3.72501, "ax":6.81079, "ay":0.11625, "alpha":-5.46173, "fx":[122.91897,97.39837,118.19895,111.90662], "fy":[16.39383,76.30477,-31.90649,-53.10401]},
-    {"t":0.30439, "x":0.91377, "y":0.71683, "heading":0.95766, "vx":1.36562, "vy":-0.04578, "omega":-3.85289, "ax":7.14507, "ay":0.31501, "alpha":-3.78478, "fx":[123.38869,109.82172,122.6923,116.62769], "fy":[10.6993,56.69091,-5.41217,-41.14501]},
-    {"t":0.3278, "x":0.9477, "y":0.71584, "heading":0.86745, "vx":1.53292, "vy":-0.03841, "omega":-3.94151, "ax":7.39525, "ay":0.32258, "alpha":-1.669, "fx":[123.41417,120.30621,123.05724,122.2978], "fy":[6.11953,27.45569,4.32432,-16.56587]},
-    {"t":0.35121, "x":0.98562, "y":0.71503, "heading":0.77516, "vx":1.70608, "vy":-0.03085, "omega":-3.98059, "ax":7.39091, "ay":0.26441, "alpha":1.5691, "fx":[122.75396,122.06351,123.31005,120.66137], "fy":[5.54545,-16.4765,3.58012,24.83754]},
-    {"t":0.37463, "x":1.0276, "y":0.71438, "heading":0.68196, "vx":1.87913, "vy":-0.02466, "omega":-3.94385, "ax":6.66668, "ay":0.41472, "alpha":5.68586, "fx":[114.37906,100.25604,123.47639,102.78147], "fy":[34.29356,-71.3271,-3.48193,67.94233]},
-    {"t":0.39804, "x":1.07342, "y":0.71392, "heading":0.58961, "vx":2.03522, "vy":-0.01495, "omega":-3.81072, "ax":5.04247, "ay":1.1932, "alpha":10.55352, "fx":[34.01149,79.3518,122.97288,97.1414], "fy":[109.9507,-94.18626,-12.92899,76.07506]},
-    {"t":0.42146, "x":1.12246, "y":0.71389, "heading":0.50039, "vx":2.15329, "vy":0.01298, "omega":-3.56361, "ax":5.15947, "ay":0.86451, "alpha":10.54379, "fx":[43.52955,73.19687,121.86231,102.62678], "fy":[109.15056,-99.20423,-21.5301,68.75683]},
-    {"t":0.44487, "x":1.17429, "y":0.71443, "heading":0.41695, "vx":2.2741, "vy":0.03323, "omega":-3.31674, "ax":5.22432, "ay":0.55585, "alpha":10.59167, "fx":[50.85859,67.24285,120.31276,107.08943], "fy":[107.70072,-103.45151,-29.29533,61.80682]},
-    {"t":0.46829, "x":1.22897, "y":0.71536, "heading":0.33929, "vx":2.39642, "vy":0.04624, "omega":-3.06874, "ax":5.2256, "ay":0.28311, "alpha":10.74546, "fx":[55.26,61.26435,118.45599,110.60806], "fy":[106.73746,-107.19162,-36.29471,55.47205]},
-    {"t":0.4917, "x":1.28651, "y":0.71652, "heading":0.26744, "vx":2.51877, "vy":0.05287, "omega":-2.81714, "ax":5.1723, "ay":0.04982, "alpha":10.99675, "fx":[57.20026,55.13336,116.39302,113.33713], "fy":[106.58823,-110.5421,-42.60073,49.84952]},
-    {"t":0.51512, "x":1.3469, "y":0.71778, "heading":0.20148, "vx":2.63988, "vy":0.05404, "omega":-2.55966, "ax":5.07202, "ay":-0.14452, "alpha":11.33474, "fx":[57.07132,48.73594,114.20201,115.42253], "fy":[107.30279,-113.56595,-48.28063,44.98637]},
-    {"t":0.53853, "x":1.41011, "y":0.719, "heading":0.14154, "vx":2.75864, "vy":0.05065, "omega":-2.29427, "ax":4.93095, "ay":-0.30229, "alpha":11.74683, "fx":[55.17744,41.98719,111.94703,116.99061], "fy":[108.77669,-116.27851,-53.38859,40.89906]},
-    {"t":0.56194, "x":1.47605, "y":0.7201, "heading":0.08782, "vx":2.87409, "vy":0.04357, "omega":-2.01922, "ax":4.72965, "ay":-0.41914, "alpha":12.2814, "fx":[50.62734,34.41745,109.65078,118.09422], "fy":[111.34427,-118.78076,-58.02816,37.74535]},
-    {"t":0.58536, "x":1.54464, "y":0.72101, "heading":0.04055, "vx":2.98483, "vy":0.03376, "omega":-1.73166, "ax":3.9645, "ay":-0.41019, "alpha":14.04528, "fx":[21.31069,16.56353,106.42315,117.89023], "fy":[120.7816,-122.59682,-63.80715,38.49505]},
-    {"t":0.60877, "x":1.61561, "y":0.72169, "heading":0.0, "vx":3.07766, "vy":0.02416, "omega":-1.4028, "ax":4.07135, "ay":-0.58711, "alpha":13.72729, "fx":[31.69599,14.24313,104.06504,119.24948], "fy":[117.68307,-122.52931,-67.23259,33.25114]},
-    {"t":0.62809, "x":1.67582, "y":0.72204, "heading":-0.0271, "vx":3.1563, "vy":0.01282, "omega":-1.13765, "ax":4.02238, "ay":-0.68429, "alpha":13.76168, "fx":[33.42746,10.55851,102.21457,119.81478], "fy":[116.6399,-122.63074,-69.75577,30.49182]},
-    {"t":0.6474, "x":1.73754, "y":0.72216, "heading":-0.04907, "vx":3.234, "vy":-0.0004, "omega":-0.87184, "ax":3.92857, "ay":-0.78542, "alpha":13.84358, "fx":[33.25139,6.32003,99.967,120.27294], "fy":[115.67273,-122.52705,-72.55653,27.46781]},
-    {"t":0.66672, "x":1.80074, "y":0.72201, "heading":-0.06591, "vx":3.30988, "vy":-0.01557, "omega":-0.60444, "ax":3.67081, "ay":-0.90124, "alpha":14.15244, "fx":[26.30455,-0.55289,96.44167,120.57117], "fy":[115.3011,-122.06252,-76.54413,23.70343]},
-    {"t":0.68603, "x":1.86535, "y":0.72154, "heading":-0.07759, "vx":3.38078, "vy":-0.03298, "omega":-0.33107, "ax":2.75032, "ay":-1.14122, "alpha":15.22834, "fx":[-11.54156,-15.41089,88.41962,120.42208], "fy":[111.55268,-119.96284,-84.43038,17.36739]},
-    {"t":0.70535, "x":1.93117, "y":0.72069, "heading":-0.08398, "vx":3.43391, "vy":-0.05502, "omega":-0.03693, "ax":0.09133, "ay":-3.27044, "alpha":14.28725, "fx":[-109.06907,-52.1174,53.1233,114.10345], "fy":[7.79667,-106.76304,-106.78745,-10.53256]},
-    {"t":0.72467, "x":1.99751, "y":0.71902, "heading":-0.08469, "vx":3.43567, "vy":-0.11819, "omega":0.23904, "ax":-2.44533, "ay":-4.60693, "alpha":8.2892, "fx":[-108.9408,-75.50029,-0.78353,23.50593], "fy":[-21.03468,-88.20277,-114.26266,-81.17323]},
-    {"t":0.74398, "x":2.06342, "y":0.71588, "heading":-0.08008, "vx":3.38844, "vy":-0.20718, "omega":0.39915, "ax":-2.68591, "ay":-3.75734, "alpha":8.4477, "fx":[-104.78632,-77.67068,-4.62576,9.45359], "fy":[-9.09679,-80.20947,-108.03285,-51.14804]},
-    {"t":0.7633, "x":2.12837, "y":0.71117, "heading":-0.07237, "vx":3.33656, "vy":-0.27976, "omega":0.56232, "ax":-2.74222, "ay":-3.11125, "alpha":7.47231, "fx":[-93.02623,-73.17691,-8.75044,-6.39987], "fy":[-7.68515,-71.82947,-95.83757,-30.40657]},
-    {"t":0.78261, "x":2.1923, "y":0.70519, "heading":-0.06151, "vx":3.28359, "vy":-0.33985, "omega":0.70665, "ax":-1.96615, "ay":-3.09524, "alpha":4.24193, "fx":[-59.54717,-50.18275,-10.12247,-10.17646], "fy":[-28.21387,-63.67888,-73.79899,-39.00807]},
-    {"t":0.80193, "x":2.25536, "y":0.69805, "heading":-0.04786, "vx":3.24561, "vy":-0.39964, "omega":0.78859, "ax":0.29191, "ay":-2.93359, "alpha":-3.08097, "fx":[18.90019,24.18189,-12.18039,-11.59643], "fy":[-60.91481,-36.43568,-35.92254,-60.73618]},
-    {"t":0.82124, "x":2.31811, "y":0.68978, "heading":-0.03262, "vx":3.25125, "vy":-0.4563, "omega":0.72908, "ax":2.07157, "ay":-1.99428, "alpha":-7.93011, "fx":[62.70384,79.50622,-1.55952,-3.64988], "fy":[-63.93522,2.04258,7.84882,-77.84565]},
-    {"t":0.84056, "x":2.38129, "y":0.6806, "heading":-0.01854, "vx":3.29126, "vy":-0.49482, "omega":0.5759, "ax":2.92799, "ay":-1.24789, "alpha":-9.93565, "fx":[81.18971,95.38525,12.10173,4.96261], "fy":[-60.81637,19.65315,46.65108,-88.01567]},
-    {"t":0.85988, "x":2.44541, "y":0.6708, "heading":-0.00742, "vx":3.34782, "vy":-0.51893, "omega":0.38399, "ax":3.23098, "ay":-0.64884, "alpha":-11.45432, "fx":[91.65728,101.03231,13.15554,7.83185], "fy":[-57.00342,32.82833,75.94142,-94.67634]},
-    {"t":0.87919, "x":2.51068, "y":0.66066, "heading":0.0, "vx":3.41023, "vy":-0.53146, "omega":0.16274, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.56968, "ay":0.51463, "alpha":-12.84831, "fx":[107.76371,26.59588,48.59934,119.25125], "fy":[62.16224,121.39443,-114.13036,-35.39187]},
+    {"t":0.02464, "x":0.43691, "y":0.54625, "heading":1.5708, "vx":0.11261, "vy":0.01268, "omega":-0.31662, "ax":4.56934, "ay":0.51797, "alpha":-12.84452, "fx":[107.71412,26.53631,48.66989,119.2672], "fy":[62.22916,121.38882,-114.06189,-35.30079]},
+    {"t":0.04929, "x":0.44107, "y":0.54672, "heading":1.56299, "vx":0.22521, "vy":0.02545, "omega":-0.63314, "ax":4.57334, "ay":0.5033, "alpha":-12.82938, "fx":[107.91518,27.09565,48.29162,119.15007], "fy":[61.85821,121.24589,-114.16915,-35.65011]},
+    {"t":0.07393, "x":0.44801, "y":0.5475, "heading":1.54739, "vx":0.33791, "vy":0.03785, "omega":-0.9493, "ax":4.5824, "ay":0.47105, "alpha":-12.79985, "fx":[108.36131,28.28135,47.50939,118.89918], "fy":[61.04812,120.95452,-114.42378,-36.42637]},
+    {"t":0.09857, "x":0.45773, "y":0.54858, "heading":1.524, "vx":0.45084, "vy":0.04946, "omega":-1.26472, "ax":4.59746, "ay":0.42194, "alpha":-12.75101, "fx":[109.04042,30.09695,46.39861,118.51154], "fy":[59.79667,120.49373,-114.77819,-37.60747]},
+    {"t":0.12321, "x":0.47023, "y":0.54992, "heading":1.49283, "vx":0.56413, "vy":0.05985, "omega":-1.57894, "ax":4.62027, "ay":0.35715, "alpha":-12.67466, "fx":[109.93342,32.54276,45.09615,117.98385], "fy":[58.10191,119.83205,-115.15521,-39.15933]},
+    {"t":0.14786, "x":0.48554, "y":0.55151, "heading":1.45392, "vx":0.67799, "vy":0.06866, "omega":-1.89128, "ax":4.6544, "ay":0.27877, "alpha":-12.55649, "fx":[111.01359,35.61705,43.86652,117.31601], "fy":[55.96421,118.92651,-115.42433,-41.03021]},
+    {"t":0.1725, "x":0.50366, "y":0.55328, "heading":1.40732, "vx":0.79268, "vy":0.07553, "omega":-2.20071, "ax":4.70772, "ay":0.19099, "alpha":-12.3692, "fx":[112.24573,39.3181,43.25802,116.5171], "fy":[53.38971,117.72096,-115.33913,-43.14088]},
+    {"t":0.19714, "x":0.52462, "y":0.5552, "heading":1.35308, "vx":0.9087, "vy":0.08023, "omega":-2.50552, "ax":4.79885, "ay":0.1034, "alpha":-12.05404, "fx":[113.58479,43.64778,44.51701,115.61633], "fy":[50.39628,116.14249,-114.33374,-45.36655]},
+    {"t":0.22179, "x":0.54847, "y":0.55721, "heading":1.29134, "vx":1.02695, "vy":0.08278, "omega":-2.80257, "ax":4.97659, "ay":0.0447, "alpha":-11.46207, "fx":[114.97361,48.61773,50.84594,114.68342], "fy":[47.02474,114.09426,-110.66174,-47.50103]},
+    {"t":0.24643, "x":0.57529, "y":0.55926, "heading":1.22228, "vx":1.14959, "vy":0.08388, "omega":-3.08503, "ax":5.37864, "ay":0.13995, "alpha":-10.14512, "fx":[116.33798,54.25755,71.24645,113.86781], "fy":[43.36224,111.44171,-96.36733,-49.18152]},
+    {"t":0.27107, "x":0.60525, "y":0.56137, "heading":1.14625, "vx":1.28214, "vy":0.08733, "omega":-3.33503, "ax":6.10392, "ay":0.98497, "alpha":-7.38048, "fx":[117.5747,60.60208,112.03304,113.46559], "fy":[39.59709,107.99915,-32.7217,-49.73461]},
+    {"t":0.29571, "x":0.6387, "y":0.56382, "heading":1.06407, "vx":1.43255, "vy":0.1116, "omega":-3.51691, "ax":6.13611, "ay":2.1906, "alpha":-6.21924, "fx":[118.51803,67.79297,105.39899,114.09428], "fy":[36.15665,103.42748,52.91911,-47.63086]},
+    {"t":0.32036, "x":0.67587, "y":0.56724, "heading":0.9774, "vx":1.58376, "vy":0.16559, "omega":-3.67017, "ax":6.143, "ay":2.52543, "alpha":-5.88392, "fx":[118.7891,76.50271,93.90775,117.06042], "fy":[34.20014,96.74161,74.32791,-38.25363]},
+    {"t":0.345, "x":0.71676, "y":0.57209, "heading":0.88696, "vx":1.73515, "vy":0.22782, "omega":-3.81516, "ax":6.4477, "ay":2.85695, "alpha":-3.91769, "fx":[116.66926,89.53056,98.23626,121.97435], "fy":[38.34975,83.61918,69.92119,-2.94941]},
+    {"t":0.36964, "x":0.76148, "y":0.57857, "heading":0.79294, "vx":1.89404, "vy":0.29822, "omega":-3.91171, "ax":5.39787, "ay":3.94928, "alpha":5.15299, "fx":[56.44487,117.64476,110.34531,72.54657], "fy":[102.3518,12.8163,49.51957,96.49281]},
+    {"t":0.39429, "x":0.80979, "y":0.58712, "heading":0.69655, "vx":2.02705, "vy":0.39554, "omega":-3.78472, "ax":4.65939, "ay":2.78204, "alpha":9.77417, "fx":[10.40523,106.44782,118.68933,72.60058], "fy":[113.63143,-51.87265,25.36238,96.86554]},
+    {"t":0.41893, "x":0.86116, "y":0.59771, "heading":0.60328, "vx":2.14188, "vy":0.4641, "omega":-3.54386, "ax":4.97557, "ay":1.85684, "alpha":10.03141, "fx":[27.03431,93.3291,121.6997,86.98999], "fy":[108.90436,-74.88108,4.22886,84.54781]},
+    {"t":0.44357, "x":0.91545, "y":0.60971, "heading":0.51595, "vx":2.26449, "vy":0.50986, "omega":-3.29666, "ax":5.10415, "ay":1.04421, "alpha":10.20137, "fx":[39.51343,78.92634,121.29283,97.82414], "fy":[102.19245,-91.11702,-14.18998,72.17232]},
+    {"t":0.46821, "x":0.9728, "y":0.62259, "heading":0.43471, "vx":2.39027, "vy":0.53559, "omega":-3.04527, "ax":5.09501, "ay":0.33049, "alpha":10.35259, "fx":[47.67973,64.70904,118.68115,105.88228], "fy":[94.11124,-102.48812,-29.99441,60.22802]},
+    {"t":0.49286, "x":1.03325, "y":0.63589, "heading":0.35967, "vx":2.51582, "vy":0.54374, "omega":-2.79015, "ax":4.98271, "ay":-0.31274, "alpha":10.49661, "fx":[51.49588,51.4938,114.73206,111.80359], "fy":[84.03974,-110.27599,-43.37804,48.93143]},
+    {"t":0.5175, "x":1.09676, "y":0.64919, "heading":0.29091, "vx":2.63861, "vy":0.53603, "omega":-2.53148, "ax":4.78153, "ay":-0.95247, "alpha":10.59299, "fx":[50.31681,39.74848,110.0935,116.06151], "fy":[68.58328,-115.43968,-54.54509,38.41115]},
+    {"t":0.54214, "x":1.16324, "y":0.66211, "heading":0.22853, "vx":2.75644, "vy":0.51256, "omega":-2.27044, "ax":4.46766, "ay":-1.75269, "alpha":10.49921, "fx":[41.39884,29.76847,105.29732,118.99837], "fy":[37.59898,-118.71474,-63.64605,28.85004]},
+    {"t":0.56679, "x":1.23252, "y":0.67421, "heading":0.17258, "vx":2.86654, "vy":0.46937, "omega":-2.01171, "ax":3.86928, "ay":-2.22672, "alpha":11.20927, "fx":[12.534,21.56714,101.0946,120.69424], "fy":[22.13505,-120.69545,-70.37663,21.67578]},
+    {"t":0.59143, "x":1.30434, "y":0.6851, "heading":0.123, "vx":2.96189, "vy":0.41449, "omega":-1.73548, "ax":3.88275, "ay":-0.94486, "alpha":13.17846, "fx":[17.01586,18.26497,101.22586,120.27435], "fy":[104.20394,-121.24449,-70.31805,24.87122]},
+    {"t":0.61607, "x":1.3785, "y":0.69503, "heading":0.08023, "vx":3.05757, "vy":0.39121, "omega":-1.41073, "ax":4.02301, "ay":-0.83071, "alpha":13.2183, "fx":[27.83878,16.66624,101.39078,120.16124], "fy":[110.5429,-121.49703,-70.21632,26.2325]},
+    {"t":0.64071, "x":1.45507, "y":0.70442, "heading":0.04547, "vx":3.15671, "vy":0.37074, "omega":-1.08499, "ax":4.11057, "ay":-0.82545, "alpha":13.18132, "fx":[34.88835,15.42457,101.21551,120.31916], "fy":[111.54535,-121.72035,-70.61079,26.19592]},
+    {"t":0.66536, "x":1.53411, "y":0.7133, "heading":0.01873, "vx":3.25801, "vy":0.3504, "omega":-0.76017, "ax":4.0074, "ay":-0.82696, "alpha":13.48413, "fx":[32.20379,11.9651,100.32407,120.53119], "fy":[113.75059,-122.21329,-72.0133,25.78582]},
+    {"t":0.69, "x":1.61561, "y":0.72169, "heading":0.0, "vx":3.35676, "vy":0.33002, "omega":-0.42788, "ax":3.89496, "ay":-1.56939, "alpha":12.47659, "fx":[39.47354,4.79277,91.75797,121.56397], "fy":[89.39251,-121.51154,-81.2905,9.61978]},
+    {"t":0.70847, "x":1.67829, "y":0.72752, "heading":-0.0079, "vx":3.42872, "vy":0.30102, "omega":-0.19738, "ax":2.81185, "ay":-4.7271, "alpha":8.5136, "fx":[-10.5651,-2.29196,79.93503,118.88002], "fy":[-87.0975,-120.18762,-91.22634,-14.10963]},
+    {"t":0.72695, "x":1.74212, "y":0.73227, "heading":-0.01155, "vx":3.48066, "vy":0.21369, "omega":-0.04009, "ax":0.80472, "ay":-6.57408, "alpha":3.96982, "fx":[-23.39303,-14.50816,35.19538,55.92509], "fy":[-108.90606,-115.9948,-111.81312,-98.05478]},
+    {"t":0.74542, "x":1.80656, "y":0.7351, "heading":-0.01229, "vx":3.49553, "vy":0.09224, "omega":0.03325, "ax":-0.26553, "ay":-6.69012, "alpha":1.35689, "fx":[-17.08019,-14.53024,6.25015,7.79968], "fy":[-108.74286,-111.63879,-112.35478,-109.70607]},
+    {"t":0.7639, "x":1.87109, "y":0.73566, "heading":-0.01168, "vx":3.49063, "vy":-0.03136, "omega":0.05831, "ax":-0.3446, "ay":-6.39197, "alpha":0.64594, "fx":[-11.32221,-10.54224,-0.50459,-0.42054], "fy":[-104.55736,-106.31113,-106.7698,-105.0865]},
+    {"t":0.78237, "x":1.93552, "y":0.73399, "heading":-0.0106, "vx":3.48426, "vy":-0.14944, "omega":0.07025, "ax":-0.39826, "ay":-5.88715, "alpha":0.1489, "fx":[-7.77055,-7.65864,-5.41843,-5.49072], "fy":[-96.9982,-97.54959,-97.66965,-97.122]},
+    {"t":0.80085, "x":1.99982, "y":0.73022, "heading":-0.0093, "vx":3.4769, "vy":-0.25821, "omega":0.073, "ax":-0.41936, "ay":-5.09947, "alpha":-0.15356, "fx":[-5.84232,-5.91539,-8.04114,-7.9351], "fy":[-84.76349,-83.97663,-83.85857,-84.64845]},
+    {"t":0.81932, "x":2.06398, "y":0.72458, "heading":-0.00795, "vx":3.46915, "vy":-0.35242, "omega":0.07016, "ax":-0.39348, "ay":-3.99635, "alpha":-0.23294, "fx":[-5.06332,-5.1366,-7.97394,-7.84848], "fy":[-66.93194,-65.3493,-65.21334,-66.79908]},
+    {"t":0.8378, "x":2.12801, "y":0.71739, "heading":-0.00666, "vx":3.46188, "vy":-0.42625, "omega":0.06586, "ax":-0.3147, "ay":-2.74377, "alpha":-0.17578, "fx":[-4.25065,-4.27958,-6.16453,-6.11753], "fy":[-46.11452,-44.67349,-44.61349,-46.05441]},
+    {"t":0.85627, "x":2.19191, "y":0.70905, "heading":-0.00544, "vx":3.45607, "vy":-0.47694, "omega":0.06261, "ax":-0.21344, "ay":-1.67476, "alpha":-0.10239, "fx":[-3.01991,-3.02483,-4.03969,-4.03129], "fy":[-28.15763,-27.23848,-27.22143,-28.14034]},
+    {"t":0.87475, "x":2.25572, "y":0.69995, "heading":-0.00428, "vx":3.45213, "vy":-0.50788, "omega":0.06072, "ax":-0.12815, "ay":-0.95499, "alpha":-0.05999, "fx":[-1.8315,-1.83115,-2.40642,-2.40611], "fy":[-16.07025,-15.51316,-15.50837,-16.06538]},
+    {"t":0.89322, "x":2.31948, "y":0.6904, "heading":-0.00316, "vx":3.44976, "vy":-0.52552, "omega":0.05961, "ax":-0.0628, "ay":-0.52625, "alpha":-0.06597, "fx":[-0.72637,-0.72489,-1.35057,-1.35161], "fy":[-9.01181,-8.39232,-8.38966,-9.00911]},
+    {"t":0.91169, "x":2.3832, "y":0.68061, "heading":-0.00206, "vx":3.4486, "vy":-0.53524, "omega":0.05839, "ax":0.03961, "ay":-0.28521, "alpha":-0.28141, "fx":[1.98002,1.9887,-0.6723,-0.6767], "fy":[-6.0432,-3.3921,-3.38739,-6.0391]},
+    {"t":0.93017, "x":2.44692, "y":0.67067, "heading":-0.00098, "vx":3.44933, "vy":-0.54051, "omega":0.05319, "ax":0.22362, "ay":-0.13728, "alpha":-2.7357, "fx":[16.5216,16.66974,-9.22386,-9.17874], "fy":[-15.10293,10.56429,10.72228,-15.26247]},
+    {"t":0.94864, "x":2.51068, "y":0.66066, "heading":0.0, "vx":3.45346, "vy":-0.54305, "omega":0.00265, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/OtoS.traj b/src/main/deploy/choreo/OtoS.traj
index ec1d1376..ba7f5559 100644
--- a/src/main/deploy/choreo/OtoS.traj
+++ b/src/main/deploy/choreo/OtoS.traj
@@ -3,7 +3,7 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -14,7 +14,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -51,53 +51,58 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.60553],
+  "waypoints":[0.0,1.83316],
   "samples":[
-    {"t":0.0, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.9745, "ay":4.57347, "alpha":0.01414, "fx":[98.794,98.89248,98.7642,98.66541], "fy":[75.59535,75.46689,75.63502,75.76343]},
-    {"t":0.03649, "x":0.73616, "y":0.72291, "heading":1.5708, "vx":0.21801, "vy":0.16688, "omega":0.00052, "ax":5.97271, "ay":4.57211, "alpha":0.01647, "fx":[98.76687,98.8816,98.73226,98.61711], "fy":[75.56952,75.41995,75.6158,75.76531]},
-    {"t":0.07298, "x":0.74809, "y":0.73204, "heading":1.57082, "vx":0.43594, "vy":0.33372, "omega":0.00112, "ax":5.96973, "ay":4.56984, "alpha":0.02035, "fx":[98.72166,98.86344,98.67912,98.5367], "fy":[75.52649,75.3418,75.58382,75.7684]},
-    {"t":0.10947, "x":0.76797, "y":0.74726, "heading":1.57086, "vx":0.65378, "vy":0.50047, "omega":0.00186, "ax":5.96379, "ay":4.56531, "alpha":0.02811, "fx":[98.63134,98.82713,98.57321,98.37621], "fy":[75.44051,75.18592,75.52009,75.77448]},
-    {"t":0.14596, "x":0.7958, "y":0.76856, "heading":1.57092, "vx":0.87139, "vy":0.66705, "omega":0.00289, "ax":5.94606, "ay":4.5518, "alpha":0.05128, "fx":[98.36118,98.71817,98.25857,97.89759], "fy":[75.18342,74.72165,75.33066,75.79181]},
-    {"t":0.18245, "x":0.83155, "y":0.79593, "heading":1.57103, "vx":1.08836, "vy":0.83314, "omega":0.00476, "ax":2.80651, "ay":2.14746, "alpha":4.48169, "fx":[30.13883,70.95579,61.57748,22.93283], "fy":[15.20348,10.96703,52.25575,63.59309]},
-    {"t":0.21894, "x":0.87313, "y":0.82776, "heading":1.5712, "vx":1.19077, "vy":0.9115, "omega":0.16829, "ax":-0.00021, "ay":0.00043, "alpha":6.26071, "fx":[-29.5273,29.54449,29.51888,-29.54975], "fy":[-29.53966,-29.51642,29.55457,29.52976]},
-    {"t":0.25542, "x":0.91658, "y":0.86102, "heading":1.57734, "vx":1.19076, "vy":0.91152, "omega":0.39674, "ax":-0.00002, "ay":0.00003, "alpha":5.74522, "fx":[-26.92562,27.27987,26.92487,-27.28045], "fy":[-27.2797,-26.92485,27.28062,26.92565]},
-    {"t":0.29191, "x":0.96003, "y":0.89428, "heading":1.59182, "vx":1.19076, "vy":0.91152, "omega":0.60638, "ax":0.0, "ay":0.0, "alpha":5.24054, "fx":[-24.19727,25.23668,24.19722,-25.23672], "fy":[-25.23667,-24.19722,25.23673,24.19727]},
-    {"t":0.3284, "x":1.00348, "y":0.92754, "heading":1.61395, "vx":1.19076, "vy":0.91152, "omega":0.7976, "ax":0.0, "ay":0.0, "alpha":4.74949, "fx":[-21.41849,23.35154,21.4185,-23.35154], "fy":[-23.35155,-21.4185,23.35154,21.41849]},
-    {"t":0.36489, "x":1.04693, "y":0.9608, "heading":1.64305, "vx":1.19076, "vy":0.91152, "omega":0.97091, "ax":0.0, "ay":0.0, "alpha":4.27441, "fx":[-18.65632,21.56776,18.65633,-21.56775], "fy":[-21.56777,-18.65634,21.56774,18.65631]},
-    {"t":0.40138, "x":1.09038, "y":0.99406, "heading":1.67848, "vx":1.19076, "vy":0.91152, "omega":1.12688, "ax":0.0, "ay":0.0, "alpha":3.81707, "fx":[-15.96755,19.83817,15.96757,-19.83815], "fy":[-19.83818,-15.96758,19.83815,15.96755]},
-    {"t":0.43787, "x":1.13383, "y":1.02733, "heading":1.7196, "vx":1.19076, "vy":0.91152, "omega":1.26616, "ax":0.0, "ay":0.0, "alpha":3.37858, "fx":[-13.3995,18.12539,13.39953,-18.12536], "fy":[-18.12539,-13.39953,18.12535,13.39949]},
-    {"t":0.47436, "x":1.17728, "y":1.06059, "heading":1.7658, "vx":1.19076, "vy":0.91152, "omega":1.38944, "ax":0.0, "ay":0.0, "alpha":2.95935, "fx":[-10.99106,16.40143,10.99109,-16.4014], "fy":[-16.40144,-10.9911,16.40139,10.99105]},
-    {"t":0.51085, "x":1.22073, "y":1.09385, "heading":1.8165, "vx":1.19076, "vy":0.91152, "omega":1.49743, "ax":0.0, "ay":0.0, "alpha":2.55912, "fx":[-8.77358,14.64668,8.77362,-14.64665], "fy":[-14.64669,-8.77363,14.64664,8.77358]},
-    {"t":0.54734, "x":1.26418, "y":1.12711, "heading":1.87114, "vx":1.19076, "vy":0.91152, "omega":1.59081, "ax":0.0, "ay":0.0, "alpha":2.17697, "fx":[-6.77186,12.84852,6.77188,-12.8485], "fy":[-12.84853,-6.77189,12.8485,6.77186]},
-    {"t":0.58383, "x":1.30763, "y":1.16037, "heading":1.92918, "vx":1.19076, "vy":0.91152, "omega":1.67024, "ax":0.0, "ay":0.0, "alpha":1.81147, "fx":[-5.00518,11.00025,5.00518,-11.00025], "fy":[-11.00025,-5.00519,11.00025,5.00518]},
-    {"t":0.62032, "x":1.35108, "y":1.19363, "heading":1.99013, "vx":1.19076, "vy":0.91152, "omega":1.73634, "ax":0.0, "ay":0.0, "alpha":1.46072, "fx":[-3.48827,9.09961,3.48825,-9.09963], "fy":[-9.09961,-3.48825,9.09963,3.48827]},
-    {"t":0.65681, "x":1.39453, "y":1.22689, "heading":2.05349, "vx":1.19076, "vy":0.91152, "omega":1.78964, "ax":0.0, "ay":0.0, "alpha":1.12245, "fx":[-2.23239,7.14805,2.23234,-7.14809], "fy":[-7.14804,-2.23233,7.1481,2.2324]},
-    {"t":0.6933, "x":1.43798, "y":1.26015, "heading":2.11879, "vx":1.19076, "vy":0.91152, "omega":1.8306, "ax":0.0, "ay":0.0, "alpha":0.79415, "fx":[-1.24608,5.14964,1.24601,-5.14971], "fy":[-5.14963,-1.246,5.14972,1.24609]},
-    {"t":0.72979, "x":1.48143, "y":1.29341, "heading":2.18559, "vx":1.19076, "vy":0.91152, "omega":1.85958, "ax":0.0, "ay":0.0, "alpha":0.47313, "fx":[-0.53596,3.11066,0.53587,-3.11075], "fy":[-3.11065,-0.53586,3.11076,0.53597]},
-    {"t":0.76627, "x":1.52488, "y":1.32667, "heading":2.25344, "vx":1.19076, "vy":0.91152, "omega":1.87684, "ax":0.0, "ay":0.0, "alpha":0.15656, "fx":[-0.10718,1.03892,0.10709,-1.03901], "fy":[-1.03891,-0.10707,1.03902,0.10719]},
-    {"t":0.80276, "x":1.56833, "y":1.35993, "heading":2.32193, "vx":1.19076, "vy":0.91152, "omega":1.88255, "ax":0.0, "ay":0.0, "alpha":-0.15846, "fx":[0.03618,-1.0566,-0.03626,1.05652], "fy":[1.05661,0.03627,-1.05651,-0.03617]},
-    {"t":0.83925, "x":1.61178, "y":1.39319, "heading":2.39062, "vx":1.19076, "vy":0.91152, "omega":1.87677, "ax":0.0, "ay":0.0, "alpha":-0.47484, "fx":[-0.10907,-3.16611,0.10901,3.16605], "fy":[3.16611,-0.109,-3.16605,0.10907]},
-    {"t":0.87574, "x":1.65523, "y":1.42645, "heading":2.4591, "vx":1.19076, "vy":0.91152, "omega":1.85944, "ax":0.0, "ay":0.0, "alpha":-0.79551, "fx":[-0.54522,-5.27928,0.5452,5.27926], "fy":[5.27928,-0.54519,-5.27926,0.54522]},
-    {"t":0.91223, "x":1.69868, "y":1.45971, "heading":2.52695, "vx":1.19076, "vy":0.91152, "omega":1.83042, "ax":0.0, "ay":0.0, "alpha":-1.12335, "fx":[-1.27354,-7.38555,1.27355,7.38557], "fy":[7.38555,-1.27355,-7.38557,1.27354]},
-    {"t":0.94872, "x":1.74213, "y":1.49297, "heading":2.59374, "vx":1.19076, "vy":0.91152, "omega":1.78943, "ax":0.0, "ay":0.0, "alpha":-1.46112, "fx":[-2.29389,-9.47426,2.29393,9.4743], "fy":[9.47426,-2.29393,-9.4743,2.29389]},
-    {"t":0.98521, "x":1.78558, "y":1.52623, "heading":2.65904, "vx":1.19076, "vy":0.91152, "omega":1.73611, "ax":0.0, "ay":0.0, "alpha":-1.81142, "fx":[-3.60416,-11.53506,3.60421,11.53511], "fy":[11.53505,-3.60422,-11.53512,3.60415]},
-    {"t":1.0217, "x":1.82903, "y":1.5595, "heading":2.72239, "vx":1.19076, "vy":0.91152, "omega":1.67001, "ax":0.0, "ay":0.0, "alpha":-2.17653, "fx":[-5.19939,-13.55816,5.19945,13.55822], "fy":[13.55815,-5.19946,-13.55822,5.19939]},
-    {"t":1.05819, "x":1.87248, "y":1.59276, "heading":2.78332, "vx":1.19076, "vy":0.91152, "omega":1.59059, "ax":0.0, "ay":0.0, "alpha":-2.55842, "fx":[-7.07089,-15.53524,7.07094,15.53529], "fy":[15.53523,-7.07094,-15.5353,7.07088]},
-    {"t":1.09468, "x":1.91593, "y":1.62602, "heading":2.84136, "vx":1.19076, "vy":0.91152, "omega":1.49724, "ax":0.0, "ay":0.0, "alpha":-2.95852, "fx":[-9.20499,-17.46017,9.20503,17.46021], "fy":[17.46017,-9.20503,-17.46021,9.20498]},
-    {"t":1.13117, "x":1.95938, "y":1.65928, "heading":2.896, "vx":1.19076, "vy":0.91152, "omega":1.38928, "ax":0.0, "ay":0.0, "alpha":-3.37772, "fx":[-11.58213,-19.33056,11.58216,19.33059], "fy":[19.33056,-11.58216,-19.33059,11.58213]},
-    {"t":1.16766, "x":2.00283, "y":1.69254, "heading":2.94669, "vx":1.19076, "vy":0.91152, "omega":1.26603, "ax":0.0, "ay":0.0, "alpha":-3.81627, "fx":[-14.17581,-21.14923,14.17582,21.14924], "fy":[21.14923,-14.17583,-21.14924,14.17581]},
-    {"t":1.20415, "x":2.04628, "y":1.7258, "heading":2.99289, "vx":1.19076, "vy":0.91152, "omega":1.12678, "ax":0.0, "ay":0.0, "alpha":-4.27372, "fx":[-16.95187,-22.92599,16.95188,22.926], "fy":[22.92599,-16.95188,-22.926,16.95187]},
-    {"t":1.24064, "x":2.08973, "y":1.75906, "heading":3.034, "vx":1.19076, "vy":0.91152, "omega":0.97084, "ax":0.0, "ay":0.0, "alpha":-4.74895, "fx":[-19.86811,-24.67952,19.86811,24.67953], "fy":[24.67953,-19.86811,-24.67953,19.86811]},
-    {"t":1.27712, "x":2.13318, "y":1.79232, "heading":3.06943, "vx":1.19076, "vy":0.91152, "omega":0.79755, "ax":0.0, "ay":0.0, "alpha":-5.24016, "fx":[-22.8739,-26.43868,22.87394,26.43873], "fy":[26.43868,-22.87395,-26.43873,22.87389]},
-    {"t":1.31361, "x":2.17663, "y":1.82558, "heading":3.09853, "vx":1.19076, "vy":0.91152, "omega":0.60634, "ax":0.00002, "ay":-0.00002, "alpha":-5.74499, "fx":[-25.91009,-28.24344,25.91065,28.24415], "fy":[28.24342,-25.91082,-28.24418,25.90992]},
-    {"t":1.3501, "x":2.22008, "y":1.85884, "heading":3.12066, "vx":1.19076, "vy":0.91152, "omega":0.39671, "ax":0.00019, "ay":-0.00041, "alpha":-6.26062, "fx":[-28.90742,-30.14259,28.91235,30.15048], "fy":[30.14015,-28.91707,-30.15291,28.9027]},
-    {"t":1.38659, "x":2.26353, "y":1.8921, "heading":3.13513, "vx":1.19076, "vy":0.9115, "omega":0.16826, "ax":-2.80649, "ay":-2.14748, "alpha":-4.48101, "fx":[-70.88449,-61.71494,-23.07689,-29.92726], "fy":[-10.81428,-52.1031,-63.66827,-15.43546]},
-    {"t":1.42308, "x":2.30511, "y":1.92393, "heading":3.14127, "vx":1.08836, "vy":0.83314, "omega":0.00476, "ax":-5.94607, "ay":-4.55179, "alpha":-0.05128, "fx":[-98.71822,-98.2588,-97.89763,-98.36104], "fy":[-74.72158,-75.33036,-75.79177,-75.1836]},
-    {"t":1.45957, "x":2.34087, "y":1.9513, "heading":3.14145, "vx":0.87139, "vy":0.66705, "omega":0.00288, "ax":-5.96379, "ay":-4.56531, "alpha":-0.02811, "fx":[-98.82714,-98.57328,-98.37624,-98.63132], "fy":[-75.1859,-75.51999,-75.77444,-75.44054]},
-    {"t":1.49606, "x":2.36869, "y":1.9726, "heading":3.14155, "vx":0.65378, "vy":0.50047, "omega":0.00186, "ax":-5.96973, "ay":-4.56984, "alpha":-0.02035, "fx":[-98.86345,-98.67915,-98.53673,-98.72167], "fy":[-75.3418,-75.58378,-75.76836,-75.52648]},
-    {"t":1.53255, "x":2.38857, "y":1.98782, "heading":-3.14157, "vx":0.43594, "vy":0.33372, "omega":0.00112, "ax":-5.97271, "ay":-4.5721, "alpha":-0.01647, "fx":[-98.88159,-98.73227,-98.61714,-98.76688], "fy":[-75.41995,-75.61578,-75.76527,-75.56951]},
-    {"t":1.56904, "x":2.4005, "y":1.99696, "heading":-3.14153, "vx":0.21801, "vy":0.16688, "omega":0.00052, "ax":-5.9745, "ay":-4.57347, "alpha":-0.01413, "fx":[-98.89248,-98.76421,-98.66544,-98.79402], "fy":[-75.4669,-75.63501,-75.76339,-75.59533]},
-    {"t":1.60553, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.05304, "ay":4.46965, "alpha":0.00958, "fx":[100.08879,100.1526,100.06644,100.00249], "fy":[73.8833,73.79707,73.91405,74.00027]},
+    {"t":0.03741, "x":0.43976, "y":0.54922, "heading":1.5708, "vx":0.22645, "vy":0.16722, "omega":0.00036, "ax":6.05131, "ay":4.46837, "alpha":0.01117, "fx":[100.06195,100.13628,100.03599,99.96145], "fy":[73.8596,73.7592,73.89547,73.99586]},
+    {"t":0.07482, "x":0.45246, "y":0.5586, "heading":1.57081, "vx":0.45284, "vy":0.33438, "omega":0.00078, "ax":6.04842, "ay":4.46624, "alpha":0.0138, "fx":[100.01721,100.1091,99.98527,99.89308], "fy":[73.82011,73.69612,73.86453,73.98849]},
+    {"t":0.11223, "x":0.47364, "y":0.57424, "heading":1.57084, "vx":0.67912, "vy":0.50147, "omega":0.00129, "ax":6.04265, "ay":4.46198, "alpha":0.01907, "fx":[99.92774,100.05473,99.88402,99.75646], "fy":[73.74113,73.57013,73.80275,73.9737]},
+    {"t":0.14965, "x":0.50327, "y":0.59612, "heading":1.57089, "vx":0.90519, "vy":0.6684, "omega":0.00201, "ax":6.02538, "ay":4.44925, "alpha":0.03486, "fx":[99.65958,99.8918,99.58199,99.34787], "fy":[73.50449,73.19372,73.61838,73.92901]},
+    {"t":0.18706, "x":0.54135, "y":0.62424, "heading":1.57096, "vx":1.1306, "vy":0.83486, "omega":0.00331, "ax":2.02477, "ay":1.49491, "alpha":4.13182, "fx":[15.80503,54.89752,49.70402,13.49903], "fy":[5.00578,4.10691,41.70653,48.04449]},
+    {"t":0.22447, "x":0.58507, "y":0.65652, "heading":1.57109, "vx":1.20635, "vy":0.89078, "omega":0.15789, "ax":-0.00002, "ay":0.00012, "alpha":4.68042, "fx":[-22.07416,22.0862,22.07316,-22.08672], "fy":[-22.08446,-22.07171,22.08845,22.0756]},
+    {"t":0.26188, "x":0.6302, "y":0.68985, "heading":1.57699, "vx":1.20635, "vy":0.89079, "omega":0.33299, "ax":0.0, "ay":0.00001, "alpha":4.26961, "fx":[-20.01692,20.26642,20.01678,-20.26654], "fy":[-20.26639,-20.01677,20.26657,20.01694]},
+    {"t":0.29929, "x":0.67533, "y":0.72317, "heading":1.58945, "vx":1.20635, "vy":0.89079, "omega":0.49272, "ax":0.0, "ay":0.0, "alpha":3.88009, "fx":[-17.95983,18.64269,17.95982,-18.6427], "fy":[-18.64269,-17.95982,18.6427,17.95983]},
+    {"t":0.3367, "x":0.72046, "y":0.7565, "heading":1.60788, "vx":1.20635, "vy":0.89079, "omega":0.63788, "ax":0.0, "ay":0.0, "alpha":3.5123, "fx":[-15.94361,17.17237,15.94361,-17.17237], "fy":[-17.17237,-15.94361,17.17237,15.94361]},
+    {"t":0.37411, "x":0.76559, "y":0.78982, "heading":1.63175, "vx":1.20635, "vy":0.89079, "omega":0.76928, "ax":0.0, "ay":0.0, "alpha":3.16628, "fx":[-13.99943,15.81919,13.99944,-15.81919], "fy":[-15.81919,-13.99944,15.81919,13.99943]},
+    {"t":0.41153, "x":0.81073, "y":0.82315, "heading":1.66053, "vx":1.20635, "vy":0.89079, "omega":0.88774, "ax":0.0, "ay":0.0, "alpha":2.84169, "fx":[-12.15053,14.55316,12.15054,-14.55316], "fy":[-14.55317,-12.15054,14.55315,12.15053]},
+    {"t":0.44894, "x":0.85586, "y":0.85648, "heading":1.69374, "vx":1.20635, "vy":0.89079, "omega":0.99405, "ax":0.0, "ay":0.0, "alpha":2.53783, "fx":[-10.41372,13.35015,10.41373,-13.35014], "fy":[-13.35015,-10.41373,13.35013,10.41371]},
+    {"t":0.48635, "x":0.90099, "y":0.8898, "heading":1.73093, "vx":1.20635, "vy":0.89079, "omega":1.08899, "ax":0.0, "ay":0.0, "alpha":2.25371, "fx":[-8.80068,12.1912,8.80069,-12.19118], "fy":[-12.1912,-8.80069,12.19118,8.80067]},
+    {"t":0.52376, "x":0.94612, "y":0.92313, "heading":1.77167, "vx":1.20635, "vy":0.89079, "omega":1.17331, "ax":0.0, "ay":0.0, "alpha":1.98811, "fx":[-7.31905,11.06175,7.31906,-11.06174], "fy":[-11.06175,-7.31907,11.06173,7.31905]},
+    {"t":0.56117, "x":0.99125, "y":0.95645, "heading":1.81556, "vx":1.20635, "vy":0.89079, "omega":1.24769, "ax":0.0, "ay":0.0, "alpha":1.73963, "fx":[-5.97338,9.9509,5.97339,-9.95089], "fy":[-9.9509,-5.97339,9.95089,5.97337]},
+    {"t":0.59858, "x":1.03638, "y":0.98978, "heading":1.86224, "vx":1.20635, "vy":0.89079, "omega":1.31277, "ax":0.0, "ay":0.0, "alpha":1.50671, "fx":[-4.76581,8.85058,4.76582,-8.85058], "fy":[-8.85059,-4.76582,8.85057,4.76581]},
+    {"t":0.636, "x":1.08151, "y":1.0231, "heading":1.91135, "vx":1.20635, "vy":0.89079, "omega":1.36914, "ax":0.0, "ay":0.0, "alpha":1.28771, "fx":[-3.69687,7.75503,3.69687,-7.75503], "fy":[-7.75503,-3.69687,7.75503,3.69687]},
+    {"t":0.67341, "x":1.12665, "y":1.05643, "heading":1.96258, "vx":1.20635, "vy":0.89079, "omega":1.41731, "ax":0.0, "ay":0.0, "alpha":1.08094, "fx":[-2.76588,6.66011,2.76588,-6.66012], "fy":[-6.66011,-2.76588,6.66012,2.76588]},
+    {"t":0.71082, "x":1.17178, "y":1.08976, "heading":2.0156, "vx":1.20635, "vy":0.89079, "omega":1.45775, "ax":0.0, "ay":0.0, "alpha":0.88465, "fx":[-1.97157,5.563,1.97155,-5.56301], "fy":[-5.563,-1.97155,5.56301,1.97157]},
+    {"t":0.74823, "x":1.21691, "y":1.12308, "heading":2.07014, "vx":1.20635, "vy":0.89079, "omega":1.49085, "ax":0.0, "ay":0.0, "alpha":0.69709, "fx":[-1.31232,4.46173,1.3123,-4.46175], "fy":[-4.46173,-1.31229,4.46175,1.31232]},
+    {"t":0.78564, "x":1.26204, "y":1.15641, "heading":2.12591, "vx":1.20635, "vy":0.89079, "omega":1.51693, "ax":0.0, "ay":0.0, "alpha":0.51652, "fx":[-0.78658,3.35503,0.78655,-3.35506], "fy":[-3.35503,-0.78654,3.35506,0.78658]},
+    {"t":0.82305, "x":1.30717, "y":1.18973, "heading":2.18266, "vx":1.20635, "vy":0.89079, "omega":1.53625, "ax":0.0, "ay":0.0, "alpha":0.34118, "fx":[-0.39304,2.24205,0.39301,-2.24208], "fy":[-2.24204,-0.393,2.24208,0.39304]},
+    {"t":0.86046, "x":1.3523, "y":1.22306, "heading":2.24014, "vx":1.20635, "vy":0.89079, "omega":1.54901, "ax":0.0, "ay":0.0, "alpha":0.16936, "fx":[-0.13085,1.12226,0.13082,-1.12229], "fy":[-1.12225,-0.13082,1.12229,0.13086]},
+    {"t":0.89788, "x":1.39743, "y":1.25638, "heading":2.29809, "vx":1.20635, "vy":0.89079, "omega":1.55535, "ax":0.0, "ay":0.0, "alpha":-0.0007, "fx":[0.00026,-0.00465,-0.00028,0.00462], "fy":[0.00466,0.00029,-0.00462,-0.00025]},
+    {"t":0.93529, "x":1.44257, "y":1.28971, "heading":2.35627, "vx":1.20635, "vy":0.89079, "omega":1.55532, "ax":0.0, "ay":0.0, "alpha":-0.1707, "fx":[-0.0001,-1.13884,0.00008,1.13882], "fy":[1.13884,-0.00008,-1.13881,0.00011]},
+    {"t":0.9727, "x":1.4877, "y":1.32304, "heading":2.41446, "vx":1.20635, "vy":0.89079, "omega":1.54894, "ax":0.0, "ay":0.0, "alpha":-0.34238, "fx":[-0.13303,-2.28037,0.13301,2.28036], "fy":[2.28037,-0.13301,-2.28035,0.13303]},
+    {"t":1.01011, "x":1.53283, "y":1.35636, "heading":2.47241, "vx":1.20635, "vy":0.89079, "omega":1.53613, "ax":0.0, "ay":0.0, "alpha":-0.51749, "fx":[-0.40033,-3.42923,0.40033,3.42923], "fy":[3.42923,-0.40033,-3.42922,0.40034]},
+    {"t":1.04752, "x":1.57796, "y":1.38969, "heading":2.52988, "vx":1.20635, "vy":0.89079, "omega":1.51677, "ax":0.0, "ay":0.0, "alpha":-0.69779, "fx":[-0.80451,-4.58535,0.80451,4.58535], "fy":[4.58535,-0.80451,-4.58535,0.80451]},
+    {"t":1.08493, "x":1.62309, "y":1.42301, "heading":2.58662, "vx":1.20635, "vy":0.89079, "omega":1.49066, "ax":0.0, "ay":0.0, "alpha":-0.88505, "fx":[-1.34859,-5.7486,1.3486,5.74861], "fy":[5.7486,-1.3486,-5.74861,1.34859]},
+    {"t":1.12234, "x":1.66822, "y":1.45634, "heading":2.64239, "vx":1.20635, "vy":0.89079, "omega":1.45755, "ax":0.0, "ay":0.0, "alpha":-1.08104, "fx":[-2.03605,-6.91888,2.03607,6.9189], "fy":[6.91888,-2.03607,-6.9189,2.03605]},
+    {"t":1.15976, "x":1.71336, "y":1.48966, "heading":2.69692, "vx":1.20635, "vy":0.89079, "omega":1.41711, "ax":0.0, "ay":0.0, "alpha":-1.28754, "fx":[-2.87051,-8.09613,2.87053,8.09615], "fy":[8.09612,-2.87053,-8.09615,2.8705]},
+    {"t":1.19717, "x":1.75849, "y":1.52299, "heading":2.74994, "vx":1.20635, "vy":0.89079, "omega":1.36894, "ax":0.0, "ay":0.0, "alpha":-1.50632, "fx":[-3.85546,-9.28053,3.85548,9.28055], "fy":[9.28053,-3.85548,-9.28056,3.85546]},
+    {"t":1.23458, "x":1.80362, "y":1.55631, "heading":2.80115, "vx":1.20635, "vy":0.89079, "omega":1.31259, "ax":0.0, "ay":0.0, "alpha":-1.73907, "fx":[-4.99388,-10.47267,4.9939,10.47269], "fy":[10.47267,-4.9939,-10.47269,4.99388]},
+    {"t":1.27199, "x":1.84875, "y":1.58964, "heading":2.85026, "vx":1.20635, "vy":0.89079, "omega":1.24753, "ax":0.0, "ay":0.0, "alpha":-1.98747, "fx":[-6.28777,-11.67393,6.28779,11.67394], "fy":[11.67392,-6.28779,-11.67394,6.28777]},
+    {"t":1.3094, "x":1.89388, "y":1.62297, "heading":2.89693, "vx":1.20635, "vy":0.89079, "omega":1.17317, "ax":0.0, "ay":0.0, "alpha":-2.25303, "fx":[-7.73761,-12.88684,7.73762,12.88685], "fy":[12.88684,-7.73762,-12.88685,7.73761]},
+    {"t":1.34681, "x":1.93901, "y":1.65629, "heading":2.94082, "vx":1.20635, "vy":0.89079, "omega":1.08888, "ax":0.0, "ay":0.0, "alpha":-2.53718, "fx":[-9.34178,-14.11577,9.34178,14.11578], "fy":[14.11577,-9.34178,-14.11578,9.34178]},
+    {"t":1.38423, "x":1.98414, "y":1.68962, "heading":2.98156, "vx":1.20635, "vy":0.89079, "omega":0.99396, "ax":0.0, "ay":0.0, "alpha":-2.8411, "fx":[-11.09587,-15.36753,11.09587,15.36754], "fy":[15.36754,-11.09587,-15.36754,11.09587]},
+    {"t":1.42164, "x":2.02928, "y":1.72294, "heading":3.01874, "vx":1.20635, "vy":0.89079, "omega":0.88767, "ax":0.0, "ay":0.0, "alpha":-3.16577, "fx":[-12.99195,-16.65223,12.99195,16.65223], "fy":[16.65223,-12.99195,-16.65223,12.99195]},
+    {"t":1.45905, "x":2.07441, "y":1.75627, "heading":3.05195, "vx":1.20635, "vy":0.89079, "omega":0.76924, "ax":0.0, "ay":0.0, "alpha":-3.51189, "fx":[-15.01777,-17.9841,15.01777,17.9841], "fy":[17.9841,-15.01777,-17.9841,15.01777]},
+    {"t":1.49646, "x":2.11954, "y":1.78959, "heading":3.08073, "vx":1.20635, "vy":0.89079, "omega":0.63785, "ax":0.0, "ay":0.0, "alpha":-3.87978, "fx":[-17.1558,-19.38242,17.1558,19.38243], "fy":[19.38242,-17.1558,-19.38243,17.15579]},
+    {"t":1.53387, "x":2.16467, "y":1.82292, "heading":3.10459, "vx":1.20635, "vy":0.89079, "omega":0.4927, "ax":0.0, "ay":-0.00001, "alpha":-4.26941, "fx":[-19.38218,-20.87233,19.3823,20.87246], "fy":[20.87231,-19.38233,-20.87248,19.38215]},
+    {"t":1.57128, "x":2.2098, "y":1.85625, "heading":3.12302, "vx":1.20635, "vy":0.89079, "omega":0.33298, "ax":0.00002, "ay":-0.00012, "alpha":-4.68032, "fx":[-21.66557,-22.48526,21.66601,22.48618], "fy":[22.48383,-21.66773,-22.4876,21.66385]},
+    {"t":1.60869, "x":2.25493, "y":1.88957, "heading":3.13548, "vx":1.20635, "vy":0.89078, "omega":0.15788, "ax":-2.02477, "ay":-1.49491, "alpha":-4.13163, "fx":[-54.81192,-49.81873,-13.62511,-15.64959], "fy":[-3.97869,-41.5823,-48.13548,-5.16757]},
+    {"t":1.64611, "x":2.29865, "y":1.92185, "heading":3.14139, "vx":1.13061, "vy":0.83486, "omega":0.00331, "ax":-6.02538, "ay":-4.44925, "alpha":-0.03486, "fx":[-99.89182,-99.58208,-99.34786,-99.65951], "fy":[-73.19369,-73.61826,-73.92901,-73.50459]},
+    {"t":1.68352, "x":2.33673, "y":1.94997, "heading":3.14151, "vx":0.90519, "vy":0.6684, "omega":0.00201, "ax":-6.04265, "ay":-4.46198, "alpha":-0.01907, "fx":[-100.05474,-99.88405,-99.75646,-99.92772], "fy":[-73.57012,-73.80272,-73.9737,-73.74115]},
+    {"t":1.72093, "x":2.36636, "y":1.97185, "heading":3.14159, "vx":0.67912, "vy":0.50147, "omega":0.00129, "ax":-6.04842, "ay":-4.46624, "alpha":-0.0138, "fx":[-100.1091,-99.98527,-99.89308,-100.01721], "fy":[-73.69611,-73.86452,-73.98849,-73.8201]},
+    {"t":1.75834, "x":2.38754, "y":1.98749, "heading":-3.14155, "vx":0.45284, "vy":0.33438, "omega":0.00078, "ax":-6.05131, "ay":-4.46837, "alpha":-0.01117, "fx":[-100.13629,-100.03598,-99.96145,-100.06195], "fy":[-73.75919,-73.89547,-73.99586,-73.8596]},
+    {"t":1.79575, "x":2.40025, "y":1.99687, "heading":-3.14152, "vx":0.22645, "vy":0.16722, "omega":0.00036, "ax":-6.05304, "ay":-4.46965, "alpha":-0.00958, "fx":[-100.1526,-100.06644,-100.0025,-100.0888], "fy":[-73.79707,-73.91406,-74.00027,-73.88329]},
+    {"t":1.83316, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/PRtoO.traj b/src/main/deploy/choreo/PRtoO.traj
index a9058f31..88ffc7e4 100644
--- a/src/main/deploy/choreo/PRtoO.traj
+++ b/src/main/deploy/choreo/PRtoO.traj
@@ -4,8 +4,8 @@
  "snapshot":{
   "waypoints":[
     {"x":3.7242095470428462, "y":0.7309682965278625, "heading":1.5707963452742666, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":2.3072574138641357, "y":0.7420299053192139, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":2.3072574138641357, "y":0.7420299053192139, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -17,8 +17,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"PRT.x", "val":3.7242095470428462}, "y":{"exp":"PRT.y", "val":0.7309682965278625}, "heading":{"exp":"PRT.heading", "val":1.5707963452742666}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"2.3072574138641357 m", "val":2.3072574138641357}, "y":{"exp":"0.7420299053192139 m", "val":0.7420299053192139}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"2.3072574138641357 m", "val":2.3072574138641357}, "y":{"exp":"0.7420299053192139 m", "val":0.7420299053192139}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -55,46 +55,48 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,0.64248,1.36762],
+  "waypoints":[0.0,0.64251,1.46974],
   "samples":[
-    {"t":0.0, "x":3.72421, "y":0.73097, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52407, "ay":0.06728, "alpha":0.0, "fx":[-124.39873,-124.39873,-124.39873,-124.39873], "fy":[1.11229,1.11229,1.11229,1.11229]},
-    {"t":0.03569, "x":3.71942, "y":0.73101, "heading":1.5708, "vx":-0.26856, "vy":0.0024, "omega":0.0, "ax":-7.52333, "ay":0.06727, "alpha":0.0, "fx":[-124.38661,-124.38661,-124.38661,-124.38661], "fy":[1.11213,1.11213,1.11213,1.11213]},
-    {"t":0.07139, "x":3.70504, "y":0.73114, "heading":1.5708, "vx":-0.53709, "vy":0.0048, "omega":0.0, "ax":-7.52247, "ay":0.06725, "alpha":0.0, "fx":[-124.37226,-124.37226,-124.37226,-124.37226], "fy":[1.11195,1.11195,1.11195,1.11195]},
-    {"t":0.10708, "x":3.68108, "y":0.73135, "heading":1.5708, "vx":-0.80559, "vy":0.0072, "omega":0.0, "ax":-7.52142, "ay":0.06724, "alpha":0.0, "fx":[-124.35499,-124.35499,-124.35499,-124.35499], "fy":[1.11172,1.11172,1.11172,1.11172]},
-    {"t":0.14277, "x":3.64753, "y":0.73165, "heading":1.5708, "vx":-1.07406, "vy":0.0096, "omega":0.0, "ax":-7.52014, "ay":0.06722, "alpha":0.0, "fx":[-124.33383,-124.33383,-124.33383,-124.33383], "fy":[1.11144,1.11144,1.11144,1.11144]},
-    {"t":0.17847, "x":3.6044, "y":0.73204, "heading":1.5708, "vx":-1.34247, "vy":0.012, "omega":0.0, "ax":-7.51854, "ay":0.0672, "alpha":0.0, "fx":[-124.30728,-124.30728,-124.30728,-124.30728], "fy":[1.1111,1.1111,1.1111,1.1111]},
-    {"t":0.21416, "x":3.5517, "y":0.73251, "heading":1.5708, "vx":-1.61083, "vy":0.0144, "omega":0.0, "ax":-7.51646, "ay":0.06718, "alpha":0.0, "fx":[-124.273,-124.273,-124.273,-124.273], "fy":[1.11065,1.11065,1.11065,1.11065]},
-    {"t":0.24985, "x":3.48941, "y":0.73307, "heading":1.5708, "vx":-1.87912, "vy":0.0168, "omega":0.0, "ax":-7.51368, "ay":0.06714, "alpha":0.0, "fx":[-124.22702,-124.22702,-124.22702,-124.22702], "fy":[1.11005,1.11005,1.11005,1.11005]},
-    {"t":0.28555, "x":3.41756, "y":0.73371, "heading":1.5708, "vx":-2.14731, "vy":0.0192, "omega":0.0, "ax":-7.50976, "ay":0.06709, "alpha":0.0, "fx":[-124.16215,-124.16215,-124.16215,-124.16215], "fy":[1.1092,1.1092,1.1092,1.1092]},
-    {"t":0.32124, "x":3.33613, "y":0.73444, "heading":1.5708, "vx":-2.41536, "vy":0.02159, "omega":0.0, "ax":-7.50381, "ay":0.06701, "alpha":0.0, "fx":[-124.06375,-124.06375,-124.06375,-124.06375], "fy":[1.10792,1.10792,1.10792,1.10792]},
-    {"t":0.35693, "x":3.24514, "y":0.73525, "heading":1.5708, "vx":-2.68319, "vy":0.02398, "omega":0.0, "ax":-7.49371, "ay":0.06688, "alpha":0.0, "fx":[-123.89689,-123.89689,-123.89689,-123.89689], "fy":[1.10574,1.10574,1.10574,1.10574]},
-    {"t":0.39263, "x":3.14459, "y":0.73615, "heading":1.5708, "vx":-2.95067, "vy":0.02637, "omega":0.0, "ax":-7.47289, "ay":0.06661, "alpha":0.0, "fx":[-123.55258,-123.55258,-123.55258,-123.55258], "fy":[1.10125,1.10125,1.10125,1.10125]},
-    {"t":0.42832, "x":3.03451, "y":0.73713, "heading":1.5708, "vx":-3.2174, "vy":0.02875, "omega":0.0, "ax":-7.40538, "ay":0.06573, "alpha":0.0, "fx":[-122.43647,-122.43647,-122.43647,-122.43647], "fy":[1.08671,1.08671,1.08671,1.08671]},
-    {"t":0.46401, "x":2.91495, "y":0.7382, "heading":1.5708, "vx":-3.48172, "vy":0.03109, "omega":0.0, "ax":-0.71029, "ay":-0.0096, "alpha":0.0, "fx":[-11.74351,-11.74351,-11.74351,-11.74351], "fy":[-0.15865,-0.15865,-0.15865,-0.15865]},
-    {"t":0.49971, "x":2.79023, "y":0.7393, "heading":1.5708, "vx":-3.50707, "vy":0.03075, "omega":0.0, "ax":-0.00082, "ay":-0.0613, "alpha":0.0, "fx":[-0.01363,-0.01363,-0.01363,-0.01363], "fy":[-1.01357,-1.01357,-1.01357,-1.01357]},
-    {"t":0.5354, "x":2.66505, "y":0.74036, "heading":1.5708, "vx":-3.5071, "vy":0.02856, "omega":0.0, "ax":0.01652, "ay":-0.26528, "alpha":0.0, "fx":[0.27318,0.27318,0.27318,0.27318], "fy":[-4.38596,-4.38596,-4.38596,-4.38596]},
-    {"t":0.57109, "x":2.53988, "y":0.74121, "heading":1.5708, "vx":-3.50651, "vy":0.01909, "omega":0.0, "ax":6.78158, "ay":-0.25484, "alpha":0.0, "fx":[112.12279,112.12279,112.12279,112.12279], "fy":[-4.21345,-4.21345,-4.21345,-4.21345]},
-    {"t":0.60678, "x":2.41904, "y":0.74173, "heading":1.5708, "vx":-3.26446, "vy":0.01, "omega":0.0, "ax":7.4341, "ay":-0.09385, "alpha":0.0, "fx":[122.9112,122.9112,122.9112,122.9112], "fy":[-1.55161,-1.55161,-1.55161,-1.55161]},
-    {"t":0.64248, "x":2.30726, "y":0.74203, "heading":1.5708, "vx":-2.99911, "vy":0.00665, "omega":0.0, "ax":0.00316, "ay":-1.12719, "alpha":0.0, "fx":[0.05223,0.05222,0.05222,0.05223], "fy":[-18.63634,-18.63634,-18.63634,-18.63634]},
-    {"t":0.68064, "x":2.1928, "y":0.74146, "heading":1.5708, "vx":-2.99899, "vy":-0.03637, "omega":0.0, "ax":0.00247, "ay":-0.18538, "alpha":0.0, "fx":[0.04078,0.04077,0.04077,0.04078], "fy":[-3.06497,-3.06497,-3.06497,-3.06497]},
-    {"t":0.71881, "x":2.07834, "y":0.73994, "heading":1.5708, "vx":-2.99889, "vy":-0.04345, "omega":0.0, "ax":0.00044, "ay":-0.02983, "alpha":0.0, "fx":[0.00724,0.00724,0.00724,0.00724], "fy":[-0.49322,-0.49322,-0.49322,-0.49322]},
-    {"t":0.75697, "x":1.96389, "y":0.73826, "heading":1.5708, "vx":-2.99888, "vy":-0.04459, "omega":0.0, "ax":0.00007, "ay":-0.0048, "alpha":0.0, "fx":[0.00118,0.00118,0.00118,0.00118], "fy":[-0.07932,-0.07932,-0.07933,-0.07933]},
-    {"t":0.79514, "x":1.84944, "y":0.73655, "heading":1.5708, "vx":-2.99887, "vy":-0.04477, "omega":0.0, "ax":0.00001, "ay":-0.00077, "alpha":0.0, "fx":[0.00019,0.00019,0.00019,0.00019], "fy":[-0.01276,-0.01276,-0.01276,-0.01276]},
-    {"t":0.8333, "x":1.73498, "y":0.73485, "heading":1.5708, "vx":-2.99887, "vy":-0.0448, "omega":0.0, "ax":0.0, "ay":-0.00012, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[-0.00205,-0.00205,-0.00205,-0.00205]},
-    {"t":0.87147, "x":1.62053, "y":0.73314, "heading":1.5708, "vx":-2.99887, "vy":-0.0448, "omega":0.0, "ax":0.0, "ay":-0.00002, "alpha":0.0, "fx":[0.0,0.00001,0.00001,0.0], "fy":[-0.00033,-0.00033,-0.00033,-0.00033]},
-    {"t":0.90963, "x":1.50608, "y":0.73143, "heading":1.5708, "vx":-2.99887, "vy":-0.0448, "omega":0.0, "ax":0.00027, "ay":0.0, "alpha":0.0, "fx":[0.00452,0.00452,0.00452,0.00452], "fy":[0.00001,0.00001,0.00002,0.00002]},
-    {"t":0.9478, "x":1.39163, "y":0.72972, "heading":1.5708, "vx":-2.99886, "vy":-0.0448, "omega":0.0, "ax":3.49455, "ay":0.05221, "alpha":0.0, "fx":[57.77687,57.77687,57.77687,57.77687], "fy":[0.8632,0.8632,0.8632,0.8632]},
-    {"t":0.98597, "x":1.27972, "y":0.72804, "heading":1.5708, "vx":-2.86549, "vy":-0.04281, "omega":0.0, "ax":7.45044, "ay":0.11131, "alpha":0.0, "fx":[123.18144,123.18144,123.18144,123.18144], "fy":[1.84038,1.84038,1.84038,1.84038]},
-    {"t":1.02413, "x":1.17578, "y":0.72649, "heading":1.5708, "vx":-2.58115, "vy":-0.03856, "omega":0.0, "ax":7.49106, "ay":0.11192, "alpha":0.0, "fx":[123.85302,123.85302,123.85302,123.85302], "fy":[1.85041,1.85041,1.85041,1.85041]},
-    {"t":1.0623, "x":1.08273, "y":0.7251, "heading":1.5708, "vx":-2.29525, "vy":-0.03429, "omega":0.0, "ax":7.50473, "ay":0.11212, "alpha":0.0, "fx":[124.07906,124.07906,124.07906,124.07906], "fy":[1.85379,1.85379,1.85379,1.85379]},
-    {"t":1.10046, "x":1.0006, "y":0.72387, "heading":1.5708, "vx":-2.00883, "vy":-0.03001, "omega":0.0, "ax":7.51159, "ay":0.11223, "alpha":0.0, "fx":[124.19243,124.19243,124.19243,124.19243], "fy":[1.85548,1.85548,1.85548,1.85548]},
-    {"t":1.13863, "x":0.9294, "y":0.72281, "heading":1.5708, "vx":-1.72215, "vy":-0.02573, "omega":0.0, "ax":7.51571, "ay":0.11229, "alpha":0.0, "fx":[124.26054,124.26054,124.26054,124.26054], "fy":[1.8565,1.8565,1.8565,1.8565]},
-    {"t":1.17679, "x":0.86915, "y":0.72191, "heading":1.5708, "vx":-1.43531, "vy":-0.02144, "omega":0.0, "ax":7.51846, "ay":0.11233, "alpha":0.0, "fx":[124.30599,124.30599,124.30599,124.30599], "fy":[1.85718,1.85718,1.85718,1.85718]},
-    {"t":1.21496, "x":0.81984, "y":0.72117, "heading":1.5708, "vx":-1.14837, "vy":-0.01716, "omega":0.0, "ax":7.52042, "ay":0.11236, "alpha":0.0, "fx":[124.33846,124.33846,124.33846,124.33846], "fy":[1.85766,1.85766,1.85766,1.85766]},
-    {"t":1.25312, "x":0.78149, "y":0.7206, "heading":1.5708, "vx":-0.86135, "vy":-0.01287, "omega":0.0, "ax":7.5219, "ay":0.11238, "alpha":0.0, "fx":[124.36283,124.36283,124.36283,124.36283], "fy":[1.85803,1.85803,1.85803,1.85803]},
-    {"t":1.29129, "x":0.7541, "y":0.72019, "heading":1.5708, "vx":-0.57427, "vy":-0.00858, "omega":0.0, "ax":7.52304, "ay":0.1124, "alpha":0.0, "fx":[124.38178,124.38178,124.38178,124.38178], "fy":[1.85831,1.85831,1.85831,1.85831]},
-    {"t":1.32945, "x":0.73766, "y":0.71994, "heading":1.5708, "vx":-0.28715, "vy":-0.00429, "omega":0.0, "ax":7.52396, "ay":0.11241, "alpha":0.0, "fx":[124.39694,124.39694,124.39694,124.39694], "fy":[1.85854,1.85854,1.85854,1.85854]},
-    {"t":1.36762, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":3.72421, "y":0.73097, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.5236, "ay":0.10723, "alpha":0.0, "fx":[-124.39106,-124.39106,-124.39106,-124.39106], "fy":[1.77293,1.77293,1.77293,1.77293]},
+    {"t":0.03569, "x":3.71942, "y":0.73104, "heading":1.5708, "vx":-0.26855, "vy":0.00383, "omega":0.0, "ax":-7.52287, "ay":0.1072, "alpha":0.0, "fx":[-124.37894,-124.37894,-124.37894,-124.37894], "fy":[1.77245,1.77245,1.77245,1.77245]},
+    {"t":0.07139, "x":3.70504, "y":0.73124, "heading":1.5708, "vx":-0.53708, "vy":0.00765, "omega":0.0, "ax":-7.522, "ay":0.10717, "alpha":0.0, "fx":[-124.36459,-124.36459,-124.36459,-124.36459], "fy":[1.77189,1.77189,1.77189,1.77189]},
+    {"t":0.10708, "x":3.68107, "y":0.73158, "heading":1.5708, "vx":-0.80558, "vy":0.01148, "omega":0.0, "ax":-7.52096, "ay":0.10713, "alpha":0.0, "fx":[-124.34733,-124.34733,-124.34733,-124.34733], "fy":[1.77122,1.77122,1.77122,1.77122]},
+    {"t":0.14278, "x":3.64753, "y":0.73206, "heading":1.5708, "vx":-1.07404, "vy":0.0153, "omega":0.0, "ax":-7.51968, "ay":0.10708, "alpha":0.0, "fx":[-124.32617,-124.32617,-124.32617,-124.32617], "fy":[1.77039,1.77039,1.77039,1.77039]},
+    {"t":0.17847, "x":3.6044, "y":0.73268, "heading":1.5708, "vx":-1.34246, "vy":0.01913, "omega":0.0, "ax":-7.51807, "ay":0.10702, "alpha":0.0, "fx":[-124.29963,-124.29963,-124.29963,-124.29963], "fy":[1.76935,1.76935,1.76935,1.76935]},
+    {"t":0.21417, "x":3.55169, "y":0.73343, "heading":1.5708, "vx":-1.61081, "vy":0.02295, "omega":0.0, "ax":-7.516, "ay":0.10694, "alpha":0.0, "fx":[-124.26535,-124.26535,-124.26535,-124.26535], "fy":[1.76801,1.76801,1.76801,1.76801]},
+    {"t":0.24986, "x":3.48941, "y":0.73431, "heading":1.5708, "vx":-1.8791, "vy":0.02676, "omega":0.0, "ax":-7.51322, "ay":0.10683, "alpha":0.0, "fx":[-124.21937,-124.21937,-124.21937,-124.21937], "fy":[1.76621,1.76621,1.76621,1.76621]},
+    {"t":0.28556, "x":3.41754, "y":0.73534, "heading":1.5708, "vx":-2.14728, "vy":0.03058, "omega":0.0, "ax":-7.5093, "ay":0.10667, "alpha":0.0, "fx":[-124.1545,-124.1545,-124.1545,-124.1545], "fy":[1.76368,1.76368,1.76368,1.76368]},
+    {"t":0.32125, "x":3.33611, "y":0.7365, "heading":1.5708, "vx":-2.41532, "vy":0.03438, "omega":0.0, "ax":-7.50334, "ay":0.10644, "alpha":0.0, "fx":[-124.0561,-124.0561,-124.0561,-124.0561], "fy":[1.75983,1.75983,1.75983,1.75983]},
+    {"t":0.35695, "x":3.24512, "y":0.73779, "heading":1.5708, "vx":-2.68316, "vy":0.03818, "omega":0.0, "ax":-7.49325, "ay":0.10605, "alpha":0.0, "fx":[-123.88922,-123.88922,-123.88922,-123.88922], "fy":[1.7533,1.7533,1.7533,1.7533]},
+    {"t":0.39264, "x":3.14457, "y":0.73922, "heading":1.5708, "vx":-2.95063, "vy":0.04197, "omega":0.0, "ax":-7.47242, "ay":0.10523, "alpha":0.0, "fx":[-123.54479,-123.54479,-123.54479,-123.54479], "fy":[1.73984,1.73984,1.73984,1.73984]},
+    {"t":0.42834, "x":3.03449, "y":0.74079, "heading":1.5708, "vx":-3.21735, "vy":0.04572, "omega":0.0, "ax":-7.40485, "ay":0.1026, "alpha":0.0, "fx":[-122.4276,-122.4276,-122.4276,-122.4276], "fy":[1.69628,1.69628,1.69628,1.69628]},
+    {"t":0.46403, "x":2.91493, "y":0.74248, "heading":1.5708, "vx":-3.48167, "vy":0.04939, "omega":0.0, "ax":-0.70684, "ay":-0.0864, "alpha":0.0, "fx":[-11.68657,-11.68657,-11.68657,-11.68657], "fy":[-1.42843,-1.42843,-1.42843,-1.42843]},
+    {"t":0.49973, "x":2.7902, "y":0.74419, "heading":1.5708, "vx":-3.5069, "vy":0.0463, "omega":0.0, "ax":-0.00448, "ay":-0.36999, "alpha":0.0, "fx":[-0.07414,-0.07414,-0.07414,-0.07414], "fy":[-6.11727,-6.11727,-6.11727,-6.11727]},
+    {"t":0.53542, "x":2.66502, "y":0.74561, "heading":1.5708, "vx":-3.50706, "vy":0.0331, "omega":0.0, "ax":0.01913, "ay":-1.53716, "alpha":0.0, "fx":[0.31625,0.31625,0.31625,0.31625], "fy":[-25.41458,-25.41458,-25.41458,-25.41458]},
+    {"t":0.57112, "x":2.53984, "y":0.74581, "heading":1.5708, "vx":-3.50638, "vy":-0.02177, "omega":0.0, "ax":6.80142, "ay":-1.07824, "alpha":0.0, "fx":[112.45094,112.45094,112.45094,112.45094], "fy":[-17.82701,-17.82701,-17.82701,-17.82701]},
+    {"t":0.60681, "x":2.41902, "y":0.74435, "heading":1.5708, "vx":-3.2636, "vy":-0.06026, "omega":0.0, "ax":7.43194, "ay":-0.26143, "alpha":0.0, "fx":[122.87562,122.87562,122.87562,122.87562], "fy":[-4.3224,-4.3224,-4.3224,-4.3224]},
+    {"t":0.64251, "x":2.30726, "y":0.74203, "heading":1.5708, "vx":-2.99832, "vy":-0.06959, "omega":0.0, "ax":0.27283, "ay":-4.93752, "alpha":0.0, "fx":[4.51085,4.51085,4.51085,4.51085], "fy":[-81.63426,-81.63426,-81.63426,-81.63426]},
+    {"t":0.6819, "x":2.18936, "y":0.73546, "heading":1.5708, "vx":-2.98757, "vy":-0.26409, "omega":0.0, "ax":0.12615, "ay":-1.30006, "alpha":0.0, "fx":[2.08566,2.08565,2.08565,2.08566], "fy":[-21.49441,-21.49441,-21.49441,-21.49441]},
+    {"t":0.72129, "x":2.07177, "y":0.72405, "heading":1.5708, "vx":-2.9826, "vy":-0.3153, "omega":0.0, "ax":0.02174, "ay":-0.20306, "alpha":0.0, "fx":[0.35946,0.35946,0.35946,0.35946], "fy":[-3.35735,-3.35735,-3.35735,-3.35735]},
+    {"t":0.76068, "x":1.9543, "y":0.71147, "heading":1.5708, "vx":-2.98175, "vy":-0.3233, "omega":0.0, "ax":0.00335, "ay":-0.03082, "alpha":0.0, "fx":[0.05536,0.05535,0.05535,0.05536], "fy":[-0.50956,-0.50956,-0.50957,-0.50957]},
+    {"t":0.80008, "x":1.83685, "y":0.69871, "heading":1.5708, "vx":-2.98161, "vy":-0.32451, "omega":0.0, "ax":0.00051, "ay":-0.00468, "alpha":0.0, "fx":[0.00842,0.00841,0.00841,0.00842], "fy":[-0.0773,-0.0773,-0.0773,-0.0773]},
+    {"t":0.83947, "x":1.7194, "y":0.68592, "heading":1.5708, "vx":-2.98159, "vy":-0.3247, "omega":0.0, "ax":0.00008, "ay":-0.00071, "alpha":0.0, "fx":[0.00128,0.00128,0.00128,0.00128], "fy":[-0.01172,-0.01172,-0.01173,-0.01173]},
+    {"t":0.87886, "x":1.60195, "y":0.67313, "heading":1.5708, "vx":-2.98159, "vy":-0.32473, "omega":0.0, "ax":0.00001, "ay":-0.00011, "alpha":0.0, "fx":[0.00019,0.00019,0.00019,0.00019], "fy":[-0.00178,-0.00178,-0.00178,-0.00178]},
+    {"t":0.91825, "x":1.4845, "y":0.66034, "heading":1.5708, "vx":-2.98159, "vy":-0.32473, "omega":0.0, "ax":0.0, "ay":-0.00002, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[-0.00027,-0.00027,-0.00027,-0.00027]},
+    {"t":0.95764, "x":1.36705, "y":0.64755, "heading":1.5708, "vx":-2.98159, "vy":-0.32473, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00001,0.00001,0.0], "fy":[-0.00004,-0.00004,-0.00004,-0.00004]},
+    {"t":0.99703, "x":1.2496, "y":0.63476, "heading":1.5708, "vx":-2.98159, "vy":-0.32473, "omega":0.0, "ax":0.00006, "ay":0.00001, "alpha":0.0, "fx":[0.00096,0.00096,0.00096,0.00096], "fy":[0.0001,0.0001,0.0001,0.0001]},
+    {"t":1.03643, "x":1.13215, "y":0.62196, "heading":1.5708, "vx":-2.98159, "vy":-0.32473, "omega":0.0, "ax":1.03407, "ay":0.11262, "alpha":0.0, "fx":[17.09679,17.09679,17.09679,17.09679], "fy":[1.86205,1.86205,1.86205,1.86205]},
+    {"t":1.07582, "x":1.0155, "y":0.60926, "heading":1.5708, "vx":-2.94085, "vy":-0.32029, "omega":0.0, "ax":7.40946, "ay":0.80698, "alpha":0.0, "fx":[122.50386,122.50386,122.50386,122.50386], "fy":[13.34217,13.34217,13.34217,13.34217]},
+    {"t":1.11521, "x":0.9054, "y":0.59727, "heading":1.5708, "vx":-2.64898, "vy":-0.28851, "omega":0.0, "ax":7.44931, "ay":0.81132, "alpha":0.0, "fx":[123.16277,123.16277,123.16277,123.16277], "fy":[13.41393,13.41393,13.41393,13.41393]},
+    {"t":1.1546, "x":0.80683, "y":0.58653, "heading":1.5708, "vx":-2.35554, "vy":-0.25655, "omega":0.0, "ax":7.46253, "ay":0.81276, "alpha":0.0, "fx":[123.38124,123.38124,123.38124,123.38124], "fy":[13.43772,13.43772,13.43772,13.43772]},
+    {"t":1.19399, "x":0.71983, "y":0.57706, "heading":1.5708, "vx":-2.06158, "vy":-0.22453, "omega":0.0, "ax":7.46911, "ay":0.81348, "alpha":0.0, "fx":[123.49016,123.49016,123.49016,123.49016], "fy":[13.44959,13.44959,13.44959,13.44959]},
+    {"t":1.23338, "x":0.64442, "y":0.56885, "heading":1.5708, "vx":-1.76736, "vy":-0.19249, "omega":0.0, "ax":7.47306, "ay":0.81391, "alpha":0.0, "fx":[123.5554,123.5554,123.5554,123.5554], "fy":[13.45669,13.45669,13.45669,13.45669]},
+    {"t":1.27278, "x":0.5806, "y":0.56189, "heading":1.5708, "vx":-1.47298, "vy":-0.16043, "omega":0.0, "ax":7.47569, "ay":0.81419, "alpha":0.0, "fx":[123.59884,123.59884,123.59884,123.59884], "fy":[13.46142,13.46142,13.46142,13.46142]},
+    {"t":1.31217, "x":0.52838, "y":0.55621, "heading":1.5708, "vx":-1.1785, "vy":-0.12835, "omega":0.0, "ax":7.47756, "ay":0.8144, "alpha":0.0, "fx":[123.62983,123.62983,123.62983,123.62983], "fy":[13.4648,13.4648,13.4648,13.4648]},
+    {"t":1.35156, "x":0.48775, "y":0.55178, "heading":1.5708, "vx":-0.88395, "vy":-0.09627, "omega":0.0, "ax":7.47897, "ay":0.81455, "alpha":0.0, "fx":[123.65306,123.65306,123.65306,123.65306], "fy":[13.46733,13.46733,13.46733,13.46733]},
+    {"t":1.39095, "x":0.45874, "y":0.54862, "heading":1.5708, "vx":-0.58934, "vy":-0.06419, "omega":0.0, "ax":7.48006, "ay":0.81467, "alpha":0.0, "fx":[123.67112,123.67112,123.67112,123.67112], "fy":[13.46929,13.46929,13.46929,13.46929]},
+    {"t":1.43034, "x":0.44132, "y":0.54673, "heading":1.5708, "vx":-0.29469, "vy":-0.03209, "omega":0.0, "ax":7.48093, "ay":0.81476, "alpha":0.0, "fx":[123.68555,123.68555,123.68555,123.68555], "fy":[13.47087,13.47087,13.47087,13.47087]},
+    {"t":1.46974, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/StoO.traj b/src/main/deploy/choreo/StoO.traj
index 5c00e2a5..aa3f6972 100644
--- a/src/main/deploy/choreo/StoO.traj
+++ b/src/main/deploy/choreo/StoO.traj
@@ -4,8 +4,8 @@
  "snapshot":{
   "waypoints":[
     {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.6131027936935425, "y":1.1535886526107788, "heading":1.9909931005444836, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":0.5329699516296387, "y":0.654477596282959, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":1.6131027936935425, "y":1.1535886526107788, "heading":1.9909931005444836, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -16,8 +16,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.6131027936935425 m", "val":1.6131027936935425}, "y":{"exp":"1.1535886526107788 m", "val":1.1535886526107788}, "heading":{"exp":"1.9909931005444836 rad", "val":1.9909931005444836}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"0.5329699516296387 m", "val":0.5329699516296387}, "y":{"exp":"0.654477596282959 m", "val":0.654477596282959}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"1.6131027936935425 m", "val":1.6131027936935425}, "y":{"exp":"1.1535886526107788 m", "val":1.1535886526107788}, "heading":{"exp":"1.9909931005444836 rad", "val":1.9909931005444836}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -53,65 +53,67 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,0.87316,1.76723],
+  "waypoints":[0.0,0.87306,1.85739],
   "samples":[
-    {"t":0.0, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.09969, "ay":-5.52861, "alpha":-0.03468, "fx":[-84.65454,-84.32988,-83.97598,-84.30159], "fy":[-91.09356,-91.3952,-91.71956,-91.41937]},
-    {"t":0.02911, "x":2.40232, "y":1.99766, "heading":-3.14151, "vx":-0.14843, "vy":-0.16091, "omega":-0.00101, "ax":-5.09814, "ay":-5.52693, "alpha":-0.03913, "fx":[-84.67233,-84.30621,-83.90676,-84.27408], "fy":[-91.0255,-91.36604,-91.73178,-91.39307]},
-    {"t":0.05821, "x":2.39584, "y":1.99063, "heading":-3.14154, "vx":-0.29681, "vy":-0.32177, "omega":-0.00215, "ax":-5.09583, "ay":-5.52441, "alpha":-0.04579, "fx":[-84.69879,-84.27098,-83.80353,-84.233], "fy":[-90.92397,-91.32259,-91.74994,-91.35382]},
-    {"t":0.08732, "x":2.38504, "y":1.97893, "heading":3.14159, "vx":-0.44513, "vy":-0.48256, "omega":-0.00348, "ax":-5.092, "ay":-5.52026, "alpha":-0.05678, "fx":[-84.74235,-84.21291,-83.63304,-84.16501], "fy":[-90.75626,-91.25096,-91.77975,-91.28889]},
-    {"t":0.11642, "x":2.36993, "y":1.96255, "heading":3.14148, "vx":-0.59333, "vy":-0.64323, "omega":-0.00513, "ax":-5.08447, "ay":-5.5121, "alpha":-0.0784, "fx":[-84.82738,-84.09917,-83.29788,-84.0309], "fy":[-90.4264,-91.11062,-91.83777,-91.16082]},
-    {"t":0.14553, "x":2.35051, "y":1.94149, "heading":3.14134, "vx":-0.74131, "vy":-0.80366, "omega":-0.00742, "ax":-5.06281, "ay":-5.4886, "alpha":-0.14055, "fx":[-85.06692,-83.77602,-82.33703,-83.64305], "fy":[-89.4797,-90.71197,-91.99966,-90.79042]},
-    {"t":0.17463, "x":2.32679, "y":1.91577, "heading":3.14112, "vx":-0.88867, "vy":-0.96341, "omega":-0.01151, "ax":-4.39735, "ay":-4.76751, "alpha":-1.98283, "fx":[-89.2032,-76.0635,-55.46775,-70.07908], "fy":[-62.38158,-81.25992,-93.94298,-77.70875]},
-    {"t":0.20374, "x":2.29906, "y":1.88571, "heading":3.14078, "vx":-1.01665, "vy":-1.10217, "omega":-0.06922, "ax":-0.00096, "ay":-0.00085, "alpha":-9.78551, "fx":[-46.1459,-46.21275,46.10639,46.1888], "fy":[46.1823,-46.13592,-46.21925,46.11637]},
-    {"t":0.23284, "x":2.26947, "y":1.85364, "heading":3.13877, "vx":-1.01668, "vy":-1.10219, "omega":-0.35403, "ax":-0.00001, "ay":0.00001, "alpha":-9.24982, "fx":[-43.51319,-43.75965,43.5128,43.75905], "fy":[43.75952,-43.51271,-43.75919,43.51328]},
-    {"t":0.26195, "x":2.23988, "y":1.82156, "heading":3.12847, "vx":-1.01668, "vy":-1.10219, "omega":-0.62324, "ax":0.0, "ay":0.0, "alpha":-8.68192, "fx":[-40.41612,-41.49138,40.41607,41.49131], "fy":[41.49137,-40.41606,-41.49132,40.41614]},
-    {"t":0.29105, "x":2.21029, "y":1.78948, "heading":3.11033, "vx":-1.01668, "vy":-1.10219, "omega":-0.87593, "ax":0.0, "ay":0.0, "alpha":-8.08177, "fx":[-36.91553,-39.29928,36.91555,39.29929], "fy":[39.29929,-36.91554,-39.29929,36.91554]},
-    {"t":0.32016, "x":2.1807, "y":1.7574, "heading":3.08483, "vx":-1.01668, "vy":-1.10219, "omega":-1.11115, "ax":0.0, "ay":0.0, "alpha":-7.4501, "fx":[-33.09564,-37.08333,33.09569,37.08338], "fy":[37.08334,-33.09569,-37.08337,33.09564]},
-    {"t":0.34926, "x":2.15111, "y":1.72532, "heading":3.05249, "vx":-1.01668, "vy":-1.10219, "omega":-1.32799, "ax":0.0, "ay":0.0, "alpha":-6.78843, "fx":[-29.04792,-34.74722,29.04801,34.74733], "fy":[34.74724,-29.04801,-34.74731,29.04792]},
-    {"t":0.37837, "x":2.12152, "y":1.69324, "heading":3.01384, "vx":-1.01668, "vy":-1.10219, "omega":-1.52557, "ax":0.0, "ay":0.0, "alpha":-6.09911, "fx":[-24.87241,-32.20401,24.87256,32.20417], "fy":[32.20403,-24.87256,-32.20415,24.87241]},
-    {"t":0.40747, "x":2.09193, "y":1.66116, "heading":2.96944, "vx":-1.01668, "vy":-1.10219, "omega":-1.70309, "ax":0.00001, "ay":-0.00001, "alpha":-5.38522, "fx":[-20.6773,-29.38129,20.67751,29.38152], "fy":[29.38132,-20.67751,-29.38149,20.6773]},
-    {"t":0.43658, "x":2.06234, "y":1.62908, "heading":2.91987, "vx":-1.01668, "vy":-1.10219, "omega":-1.85982, "ax":0.00001, "ay":-0.00001, "alpha":-4.65035, "fx":[-16.57657,-26.22546,16.57684,26.22574], "fy":[26.22549,-16.57683,-26.22571,16.57657]},
-    {"t":0.46568, "x":2.03275, "y":1.597, "heading":2.86574, "vx":-1.01668, "vy":-1.10219, "omega":-1.99517, "ax":0.00001, "ay":-0.00001, "alpha":-3.89837, "fx":[-12.68623,-22.70432,12.68651,22.70461], "fy":[22.70435,-12.6865,-22.70458,12.68624]},
-    {"t":0.49479, "x":2.00316, "y":1.56492, "heading":2.80767, "vx":-1.01668, "vy":-1.10219, "omega":-2.10864, "ax":0.00001, "ay":-0.00001, "alpha":-3.13308, "fx":[-9.11956,-18.80817,9.11978,18.80839], "fy":[18.80819,-9.11977,-18.80838,9.11957]},
-    {"t":0.52389, "x":1.97357, "y":1.53284, "heading":2.7463, "vx":-1.01668, "vy":-1.10219, "omega":-2.19982, "ax":0.0, "ay":0.0, "alpha":-2.35792, "fx":[-5.98222,-14.54916,5.98231,14.54925], "fy":[14.54917,-5.98231,-14.54924,5.98223]},
-    {"t":0.553, "x":1.94397, "y":1.50076, "heading":2.68227, "vx":-1.01668, "vy":-1.10219, "omega":-2.26845, "ax":0.0, "ay":0.0, "alpha":-1.57588, "fx":[-3.36786,-9.95967,3.36777,9.95958], "fy":[9.95967,-3.36777,-9.95958,3.36786]},
-    {"t":0.5821, "x":1.91438, "y":1.46868, "heading":2.61625, "vx":-1.01668, "vy":-1.10219, "omega":-2.31432, "ax":-0.00001, "ay":0.00001, "alpha":-0.78944, "fx":[-1.35441,-5.08988,1.35412,5.08959], "fy":[5.08987,-1.35413,-5.0896,1.3544]},
-    {"t":0.61121, "x":1.88479, "y":1.4366, "heading":2.54889, "vx":-1.01668, "vy":-1.10219, "omega":-2.33729, "ax":-0.00001, "ay":0.00001, "alpha":-0.00073, "fx":[-0.00115,-0.00499,0.00071,0.00456], "fy":[0.00498,-0.00073,-0.00457,0.00113]},
-    {"t":0.64031, "x":1.8552, "y":1.40452, "heading":2.48086, "vx":-1.01668, "vy":-1.10219, "omega":-2.33732, "ax":-0.00002, "ay":0.00002, "alpha":0.78828, "fx":[0.65362,5.21795,-0.65425,-5.21857], "fy":[-5.21798,0.65422,5.21855,-0.65365]},
-    {"t":0.66942, "x":1.82561, "y":1.37244, "heading":2.41283, "vx":-1.01668, "vy":-1.10219, "omega":-2.31437, "ax":-0.00006, "ay":0.00005, "alpha":1.57552, "fx":[0.59407,10.49346,-0.59597,-10.49533], "fy":[-10.49354,0.5959,10.49526,-0.59415]},
-    {"t":0.69852, "x":1.79602, "y":1.34036, "heading":2.34547, "vx":-1.01668, "vy":-1.10219, "omega":-2.26852, "ax":-0.00043, "ay":0.0004, "alpha":2.35866, "fx":[-0.17592,15.72816,0.16151,-15.7421], "fy":[-15.7287,-0.16208,15.74156,0.17535]},
-    {"t":0.72763, "x":1.76643, "y":1.30828, "heading":2.27945, "vx":-1.0167, "vy":-1.10218, "omega":-2.19987, "ax":-0.0039, "ay":0.0036, "alpha":3.13494, "fx":[-1.6701,20.79109,1.53712,-20.91585], "fy":[-20.79541,-1.54277,20.91153,1.66441]},
-    {"t":0.75673, "x":1.73684, "y":1.27621, "heading":2.21542, "vx":-1.01681, "vy":-1.10207, "omega":-2.10862, "ax":-0.03515, "ay":0.03246, "alpha":3.90085, "fx":[-4.26464,25.21708,3.04063,-26.31755], "fy":[-25.24647,-3.0984,26.28886,4.20263]},
-    {"t":0.78584, "x":1.70723, "y":1.24414, "heading":2.15405, "vx":-1.01783, "vy":-1.10113, "omega":-1.99509, "ax":-0.30589, "ay":0.28507, "alpha":4.6268, "fx":[-11.74897,25.56423,0.83611,-34.88095], "fy":[-25.68936,-1.24339,34.82408,10.96126]},
-    {"t":0.81495, "x":1.67747, "y":1.21222, "heading":2.09598, "vx":-1.02674, "vy":-1.09283, "omega":-1.86043, "ax":-2.20543, "ay":2.20154, "alpha":3.90645, "fx":[-49.45459,-10.13058,-27.55105,-58.71696], "fy":[9.10815,35.32272,61.38388,39.78121]},
-    {"t":0.84405, "x":1.64666, "y":1.18134, "heading":2.04183, "vx":-1.09092, "vy":-1.02876, "omega":-1.74673, "ax":-4.25511, "ay":5.16872, "alpha":0.78985, "fx":[-76.30725,-65.62335,-64.81949,-74.65665], "fy":[79.69635,88.21183,90.72357,83.19501]},
-    {"t":0.87316, "x":1.6131, "y":1.15359, "heading":1.99099, "vx":-1.21477, "vy":-0.87832, "omega":-1.72374, "ax":-3.47156, "ay":5.63195, "alpha":0.93653, "fx":[-64.77245,-50.69479,-50.76852,-63.35141], "fy":[87.47832,95.96297,98.18447,90.83651]},
-    {"t":0.90754, "x":1.56928, "y":1.12672, "heading":1.93172, "vx":-1.33415, "vy":-0.68465, "omega":-1.69153, "ax":-0.83576, "ay":1.72032, "alpha":5.34907, "fx":[-33.42472,21.99252,0.61315,-44.45282], "fy":[-4.20914,16.92181,59.45762,41.60101]},
-    {"t":0.94193, "x":1.52291, "y":1.10419, "heading":1.87355, "vx":-1.36289, "vy":-0.62549, "omega":-1.50759, "ax":-0.06629, "ay":0.14514, "alpha":5.54249, "fx":[-18.50388,31.89174,15.85395,-33.62587], "fy":[-30.38949,-14.7044,35.10662,19.58573]},
-    {"t":0.97632, "x":1.476, "y":1.08277, "heading":1.82171, "vx":-1.36517, "vy":-0.6205, "omega":-1.317, "ax":-0.00518, "ay":0.0114, "alpha":5.09939, "fx":[-17.43145,29.20598,17.22933,-29.34648], "fy":[-29.08883,-17.14052,29.46354,17.51988]},
-    {"t":1.0107, "x":1.42905, "y":1.06144, "heading":1.77642, "vx":-1.36535, "vy":-0.62011, "omega":-1.14165, "ax":-0.0004, "ay":0.00087, "alpha":4.669, "fx":[-17.07243,26.05385,17.05733,-26.06498], "fy":[-26.04497,-17.05046,26.07387,17.0793]},
-    {"t":1.04509, "x":1.3821, "y":1.04011, "heading":1.73716, "vx":-1.36536, "vy":-0.62008, "omega":-0.98109, "ax":-0.00003, "ay":0.00007, "alpha":4.25594, "fx":[-16.47604,23.12473,16.47493,-23.12559], "fy":[-23.12408,-16.47442,23.12624,16.47656]},
-    {"t":1.07948, "x":1.33515, "y":1.01879, "heading":1.70343, "vx":-1.36536, "vy":-0.62008, "omega":-0.83474, "ax":0.0, "ay":0.0, "alpha":3.86012, "fx":[-15.64223,20.45842,15.64215,-20.45849], "fy":[-20.45838,-15.64211,20.45853,15.64226]},
-    {"t":1.11387, "x":1.2882, "y":0.99747, "heading":1.67472, "vx":-1.36536, "vy":-0.62008, "omega":-0.702, "ax":0.0, "ay":0.0, "alpha":3.48119, "fx":[-14.63038,18.03766,14.63037,-18.03766], "fy":[-18.03766,-14.63037,18.03766,14.63038]},
-    {"t":1.14825, "x":1.24125, "y":0.97614, "heading":1.65058, "vx":-1.36536, "vy":-0.62008, "omega":-0.5823, "ax":0.0, "ay":0.0, "alpha":3.11855, "fx":[-13.49252,15.83758,13.49252,-15.83758], "fy":[-15.83758,-13.49252,15.83758,13.49252]},
-    {"t":1.18264, "x":1.1943, "y":0.95482, "heading":1.63056, "vx":-1.36536, "vy":-0.62008, "omega":-0.47506, "ax":0.0, "ay":0.0, "alpha":2.77139, "fx":[-12.26992,13.83163,12.26992,-13.83163], "fy":[-13.83163,-12.26992,13.83163,12.26992]},
-    {"t":1.21703, "x":1.14735, "y":0.9335, "heading":1.61422, "vx":-1.36536, "vy":-0.62008, "omega":-0.37976, "ax":0.0, "ay":0.0, "alpha":2.43871, "fx":[-10.99442,11.99329,10.99442,-11.99329], "fy":[-11.99329,-10.99442,11.99329,10.99442]},
-    {"t":1.25142, "x":1.1004, "y":0.91217, "heading":1.60116, "vx":-1.36536, "vy":-0.62008, "omega":-0.2959, "ax":0.0, "ay":0.0, "alpha":2.11936, "fx":[-9.68997,10.29709,9.68997,-10.29709], "fy":[-10.29709,-9.68997,10.29709,9.68997]},
-    {"t":1.2858, "x":1.05344, "y":0.89085, "heading":1.59099, "vx":-1.36536, "vy":-0.62008, "omega":-0.22302, "ax":0.0, "ay":0.0, "alpha":1.81203, "fx":[-8.37397,8.71914,8.37397,-8.71914], "fy":[-8.71914,-8.37397,8.71914,8.37397]},
-    {"t":1.32019, "x":1.00649, "y":0.86953, "heading":1.58332, "vx":-1.36536, "vy":-0.62008, "omega":-0.16071, "ax":0.0, "ay":0.0, "alpha":1.51531, "fx":[-7.05843,7.23745,7.05843,-7.23745], "fy":[-7.23745,-7.05843,7.23745,7.05843]},
-    {"t":1.35458, "x":0.95954, "y":0.84821, "heading":1.57779, "vx":-1.36536, "vy":-0.62008, "omega":-0.1086, "ax":0.0, "ay":0.0, "alpha":1.22769, "fx":[-5.75103,5.83206,5.75103,-5.83206], "fy":[-5.83206,-5.75103,5.83206,5.75103]},
-    {"t":1.38897, "x":0.91259, "y":0.82688, "heading":1.57406, "vx":-1.36536, "vy":-0.62008, "omega":-0.06638, "ax":0.0, "ay":0.0, "alpha":0.94762, "fx":[-4.45584,4.485,4.45584,-4.485], "fy":[-4.485,-4.45584,4.485,4.45584]},
-    {"t":1.42335, "x":0.86564, "y":0.80556, "heading":1.57177, "vx":-1.36536, "vy":-0.62008, "omega":-0.0338, "ax":0.0, "ay":0.0, "alpha":0.67347, "fx":[-3.174,3.18021,3.174,-3.18021], "fy":[-3.18021,-3.174,3.18021,3.174]},
-    {"t":1.45774, "x":0.81869, "y":0.78424, "heading":1.57061, "vx":-1.36536, "vy":-0.62008, "omega":-0.01064, "ax":0.0, "ay":0.0, "alpha":0.40357, "fx":[-1.9042,1.9035,1.9042,-1.9035], "fy":[-1.9035,-1.9042,1.9035,1.9042]},
-    {"t":1.49213, "x":0.77174, "y":0.76291, "heading":1.57025, "vx":-1.36536, "vy":-0.62008, "omega":0.00324, "ax":0.0, "ay":0.0, "alpha":0.13624, "fx":[-0.64308,0.64238,0.64308,-0.64238], "fy":[-0.64238,-0.64308,0.64238,0.64308]},
-    {"t":1.52651, "x":0.72479, "y":0.74159, "heading":1.57036, "vx":-1.36536, "vy":-0.62008, "omega":0.00793, "ax":0.00036, "ay":0.00016, "alpha":-0.13021, "fx":[0.62044,-0.60812,-0.60866,0.6199], "fy":[0.61668,0.61722,-0.61133,-0.61187]},
-    {"t":1.5609, "x":0.67784, "y":0.72027, "heading":1.57063, "vx":-1.36535, "vy":-0.62007, "omega":0.00345, "ax":5.51296, "ay":2.50372, "alpha":-0.0855, "fx":[91.18553,90.71857,91.11408,91.5748], "fy":[41.77177,42.16797,41.01476,40.62572]},
-    {"t":1.59529, "x":0.63415, "y":0.70043, "heading":1.57075, "vx":-1.17577, "vy":-0.53398, "omega":0.00051, "ax":6.81458, "ay":3.09485, "alpha":-0.00431, "fx":[112.66047,112.6462,112.67645,112.69069], "fy":[51.1869,51.21716,51.15004,51.11981]},
-    {"t":1.62968, "x":0.59774, "y":0.68389, "heading":1.57077, "vx":-0.94144, "vy":-0.42755, "omega":0.00036, "ax":6.83636, "ay":3.10474, "alpha":-0.00307, "fx":[113.02283,113.0128,113.03446,113.04448], "fy":[51.34516,51.36682,51.31895,51.2973]},
-    {"t":1.66406, "x":0.56941, "y":0.67103, "heading":1.57078, "vx":-0.70635, "vy":-0.32079, "omega":0.00025, "ax":6.84378, "ay":3.10811, "alpha":-0.00264, "fx":[113.14626,113.13765,113.15636,113.16496], "fy":[51.39906,51.41777,51.37647,51.35776]},
-    {"t":1.69845, "x":0.54917, "y":0.66183, "heading":1.57079, "vx":-0.47101, "vy":-0.21391, "omega":0.00016, "ax":6.84752, "ay":3.10981, "alpha":-0.00243, "fx":[113.20847,113.20057,113.21779,113.22568], "fy":[51.42622,51.44344,51.40545,51.38824]},
-    {"t":1.73284, "x":0.53702, "y":0.65632, "heading":1.57079, "vx":-0.23555, "vy":-0.10697, "omega":0.00008, "ax":6.84978, "ay":3.11083, "alpha":-0.0023, "fx":[113.24595,113.23848,113.2548,113.26227], "fy":[51.44259,51.45891,51.42292,51.40661]},
-    {"t":1.76723, "x":0.53297, "y":0.65448, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.10647, "ay":-5.52235, "alpha":-0.03419, "fx":[-84.76147,-84.44127,-84.09309,-84.41423], "fy":[-90.99404,-91.29224,-91.61215,-91.31534]},
+    {"t":0.0291, "x":2.40232, "y":1.99766, "heading":-3.14151, "vx":-0.14861, "vy":-0.16071, "omega":-0.001, "ax":-5.10492, "ay":-5.52067, "alpha":-0.0386, "fx":[-84.77864,-84.41749,-84.02443,-84.38676], "fy":[-90.92645,-91.26317,-91.62397,-91.28901]},
+    {"t":0.0582, "x":2.39583, "y":1.99065, "heading":-3.14154, "vx":-0.29717, "vy":-0.32138, "omega":-0.00212, "ax":-5.1026, "ay":-5.51816, "alpha":-0.04517, "fx":[-84.80419,-84.38209,-83.92202,-84.34574], "fy":[-90.82563,-91.21986,-91.64152,-91.24971]},
+    {"t":0.08731, "x":2.38502, "y":1.97896, "heading":3.14159, "vx":-0.44567, "vy":-0.48197, "omega":-0.00343, "ax":-5.09877, "ay":-5.51402, "alpha":-0.05602, "fx":[-84.84622,-84.32374,-83.75289,-84.27787], "fy":[-90.6591,-91.14846,-91.67034,-91.1847]},
+    {"t":0.11641, "x":2.36989, "y":1.9626, "heading":3.14149, "vx":-0.59405, "vy":-0.64244, "omega":-0.00506, "ax":-5.09123, "ay":-5.50586, "alpha":-0.07738, "fx":[-84.92828,-84.20945,-83.42043,-84.14398], "fy":[-90.3316,-91.00857,-91.7264,-91.05648]},
+    {"t":0.14551, "x":2.35045, "y":1.94157, "heading":3.14134, "vx":-0.74222, "vy":-0.80267, "omega":-0.00731, "ax":-5.06955, "ay":-5.4824, "alpha":-0.13874, "fx":[-85.15933,-83.88474,-82.46744,-83.75689], "fy":[-89.39184,-90.61121,-91.88278,-90.68574]},
+    {"t":0.17461, "x":2.3267, "y":1.91589, "heading":3.14113, "vx":-0.88976, "vy":-0.96222, "omega":-0.01135, "ax":-4.40686, "ay":-4.76605, "alpha":-1.95273, "fx":[-89.10561,-76.14079,-55.88923,-70.30677], "fy":[-62.59242,-81.19386,-93.72763,-77.6829]},
+    {"t":0.20372, "x":2.29894, "y":1.88587, "heading":3.1408, "vx":-1.018, "vy":-1.10092, "omega":-0.06818, "ax":-0.00096, "ay":-0.00086, "alpha":-9.71825, "fx":[-45.82934,-45.89458,45.78997,45.87072], "fy":[45.86409,-45.81966,-45.90121,45.79965]},
+    {"t":0.23282, "x":2.26932, "y":1.85383, "heading":3.13881, "vx":-1.01803, "vy":-1.10094, "omega":-0.351, "ax":-0.00001, "ay":0.00001, "alpha":-9.18537, "fx":[-43.21182,-43.45287,43.21147,43.45232], "fy":[43.45275,-43.21138,-43.45244,43.2119]},
+    {"t":0.26192, "x":2.23969, "y":1.82179, "heading":3.1286, "vx":-1.01803, "vy":-1.10094, "omega":-0.61832, "ax":0.0, "ay":0.0, "alpha":-8.62161, "fx":[-40.14077,-41.19786,40.14072,41.19778], "fy":[41.19784,-40.14071,-41.1978,40.14078]},
+    {"t":0.29102, "x":2.21006, "y":1.78975, "heading":3.1106, "vx":-1.01803, "vy":-1.10094, "omega":-0.86922, "ax":0.0, "ay":0.0, "alpha":-8.02713, "fx":[-36.67672,-39.02339,36.67674,39.0234], "fy":[39.0234,-36.67673,-39.0234,36.67673]},
+    {"t":0.32012, "x":2.18044, "y":1.75771, "heading":3.08531, "vx":-1.01803, "vy":-1.10094, "omega":-1.10283, "ax":0.0, "ay":0.0, "alpha":-7.40283, "fx":[-32.90312,-36.83239,32.90316,36.83244], "fy":[36.8324,-32.90316,-36.83243,32.90312]},
+    {"t":0.34923, "x":2.15081, "y":1.72567, "heading":3.05321, "vx":-1.01803, "vy":-1.10094, "omega":-1.31827, "ax":0.0, "ay":0.0, "alpha":-6.75038, "fx":[-28.90997,-34.53162,28.91005,34.53172], "fy":[34.53164,-28.91005,-34.5317,28.90997]},
+    {"t":0.37833, "x":2.12118, "y":1.69363, "heading":3.01485, "vx":-1.01803, "vy":-1.10094, "omega":-1.51472, "ax":0.0, "ay":0.0, "alpha":-6.07222, "fx":[-24.79505,-32.03707,24.79519,32.03723], "fy":[32.0371,-24.7952,-32.03721,24.79505]},
+    {"t":0.40743, "x":2.09155, "y":1.66159, "heading":2.97077, "vx":-1.01803, "vy":-1.10094, "omega":-1.69143, "ax":0.00001, "ay":-0.00001, "alpha":-5.37147, "fx":[-20.66341,-29.27888,20.66362,29.27911], "fy":[29.27891,-20.66362,-29.27908,20.66341]},
+    {"t":0.43653, "x":2.06193, "y":1.62955, "heading":2.92154, "vx":-1.01803, "vy":-1.10094, "omega":-1.84776, "ax":0.00001, "ay":-0.00001, "alpha":-4.65168, "fx":[-16.62513,-26.20516,16.62539,26.20543], "fy":[26.20519,-16.62539,-26.2054,16.62513]},
+    {"t":0.46563, "x":2.0323, "y":1.59751, "heading":2.86777, "vx":-1.01803, "vy":-1.10094, "omega":-1.98313, "ax":0.00001, "ay":-0.00001, "alpha":-3.91657, "fx":[-12.79171,-22.78442,12.79199,22.7847], "fy":[22.78444,-12.79198,-22.78467,12.79172]},
+    {"t":0.49474, "x":2.00267, "y":1.56547, "heading":2.81005, "vx":-1.01803, "vy":-1.10094, "omega":-2.09711, "ax":0.00001, "ay":-0.00001, "alpha":-3.16977, "fx":[-9.27172,-19.00636,9.27194,19.00659], "fy":[19.00638,-9.27194,-19.00657,9.27173]},
+    {"t":0.52384, "x":1.97305, "y":1.53343, "heading":2.74902, "vx":-1.01803, "vy":-1.10095, "omega":-2.18936, "ax":0.0, "ay":0.0, "alpha":-2.41445, "fx":[-6.16626,-14.88125,6.16637,14.88135], "fy":[14.88125,-6.16636,-14.88134,6.16627]},
+    {"t":0.55294, "x":1.94342, "y":1.50139, "heading":2.68531, "vx":-1.01803, "vy":-1.10095, "omega":-2.25962, "ax":0.0, "ay":0.0, "alpha":-1.6533, "fx":[-3.56505,-10.43822,3.56498,10.43814], "fy":[10.43821,-3.56498,-10.43814,3.56505]},
+    {"t":0.58204, "x":1.91379, "y":1.46935, "heading":2.61955, "vx":-1.01803, "vy":-1.10095, "omega":-2.30774, "ax":-0.00001, "ay":0.00001, "alpha":-0.88843, "fx":[-1.54313,-5.72303,1.54286,5.72277], "fy":[5.72303,-1.54287,-5.72278,1.54312]},
+    {"t":0.61115, "x":1.88417, "y":1.43731, "heading":2.55239, "vx":-1.01803, "vy":-1.10094, "omega":-2.33359, "ax":-0.00001, "ay":0.00001, "alpha":-0.12154, "fx":[-0.15828,-0.79553,0.15786,0.79511], "fy":[0.79551,-0.15788,-0.79512,0.15826]},
+    {"t":0.64025, "x":1.85454, "y":1.40527, "heading":2.48448, "vx":-1.01803, "vy":-1.10094, "omega":-2.33713, "ax":-0.00002, "ay":0.00002, "alpha":0.64587, "fx":[0.55096,4.27328,-0.55154,-4.27385], "fy":[-4.2733,0.55151,4.27383,-0.55099]},
+    {"t":0.66935, "x":1.82491, "y":1.37323, "heading":2.41646, "vx":-1.01803, "vy":-1.10094, "omega":-2.31833, "ax":-0.00004, "ay":0.00004, "alpha":1.41225, "fx":[0.56678,9.40415,-0.5682,-9.40556], "fy":[-9.4042,0.56815,9.4055,-0.56683]},
+    {"t":0.69845, "x":1.79529, "y":1.34119, "heading":2.34899, "vx":-1.01803, "vy":-1.10094, "omega":-2.27723, "ax":-0.00029, "ay":0.00027, "alpha":2.1758, "fx":[-0.10948,14.51089,0.0996,-14.52051], "fy":[-14.51126,-0.09997,14.52014,0.1091]},
+    {"t":0.72755, "x":1.76566, "y":1.30915, "heading":2.28272, "vx":-1.01804, "vy":-1.10094, "omega":-2.21391, "ax":-0.00267, "ay":0.00247, "alpha":2.93431, "fx":[-1.48239,19.48083,1.39176,-19.56656], "fy":[-19.48376,-1.39546,19.56363,1.47867]},
+    {"t":0.75666, "x":1.73603, "y":1.27711, "heading":2.21829, "vx":-1.01812, "vy":-1.10086, "omega":-2.12852, "ax":-0.02423, "ay":0.02242, "alpha":3.68492, "fx":[-3.79953,23.96921,2.96048,-24.73245], "fy":[-23.98994,-2.99871,24.71204,3.7594]},
+    {"t":0.78576, "x":1.70639, "y":1.24508, "heading":2.15635, "vx":-1.01883, "vy":-1.10021, "omega":-2.02128, "ax":-0.21342, "ay":0.19876, "alpha":4.41232, "fx":[-9.66932,25.56746,2.10647,-32.11882], "fy":[-25.66634,-2.42069,32.05279,9.17903]},
+    {"t":0.81486, "x":1.67665, "y":1.21315, "heading":2.09752, "vx":-1.02504, "vy":-1.09443, "omega":-1.89287, "ax":-1.66786, "ay":1.63475, "alpha":4.30191, "fx":[-39.46508,1.52707,-19.48422,-52.87934], "fy":[-2.1035,23.2569,54.52944,32.42919]},
+    {"t":0.84396, "x":1.64611, "y":1.18199, "heading":2.04244, "vx":-1.07357, "vy":-1.04685, "omega":-1.76768, "ax":-4.17283, "ay":4.87272, "alpha":1.01604, "fx":[-76.21874,-62.99473,-62.38806,-74.36347], "fy":[72.96371,83.71297,87.41545,78.15915]},
+    {"t":0.87306, "x":1.6131, "y":1.15359, "heading":1.99099, "vx":-1.19501, "vy":-0.90505, "omega":-1.73811, "ax":-3.38834, "ay":5.19686, "alpha":1.34192, "fx":[-65.88446,-46.67336,-47.47173,-64.05441], "fy":[77.33997,89.32305,93.45936,83.56586]},
+    {"t":0.90822, "x":1.569, "y":1.12498, "heading":1.92989, "vx":-1.31413, "vy":-0.72235, "omega":-1.69093, "ax":-0.5758, "ay":1.08411, "alpha":5.58556, "fx":[-27.73488,26.45369,5.11947,-41.91825], "fy":[-16.21228,4.00174,51.14275,32.76406]},
+    {"t":0.94337, "x":1.52245, "y":1.10026, "heading":1.87045, "vx":-1.33437, "vy":-0.68424, "omega":-1.49458, "ax":-0.04293, "ay":0.08394, "alpha":5.40003, "fx":[-17.66477,31.28054,15.98647,-32.44126], "fy":[-30.48672,-15.40971,33.2295,18.21846]},
+    {"t":0.97853, "x":1.47551, "y":1.07626, "heading":1.81791, "vx":-1.33588, "vy":-0.68129, "omega":-1.30474, "ax":-0.00316, "ay":0.0062, "alpha":4.96016, "fx":[-17.02573,28.36843,16.90495,-28.45672], "fy":[-28.31025,-16.8626,28.51488,17.06797]},
+    {"t":1.01368, "x":1.42855, "y":1.05231, "heading":1.77204, "vx":-1.33599, "vy":-0.68107, "omega":-1.13037, "ax":-0.00023, "ay":0.00045, "alpha":4.53799, "fx":[-16.70114,25.252,16.69264,-25.25855], "fy":[-25.24786,-16.68954,25.26269,16.70424]},
+    {"t":1.04884, "x":1.38158, "y":1.02837, "heading":1.7323, "vx":-1.336, "vy":-0.68106, "omega":-0.97084, "ax":-0.00002, "ay":0.00003, "alpha":4.13502, "fx":[-16.11671,22.38979,16.11613,-22.39026], "fy":[-22.3895,-16.11591,22.39054,16.11693]},
+    {"t":1.08399, "x":1.33461, "y":1.00443, "heading":1.69817, "vx":-1.336, "vy":-0.68105, "omega":-0.82547, "ax":0.0, "ay":0.0, "alpha":3.75097, "fx":[-15.30415,19.79982,15.30411,-19.79986], "fy":[-19.79981,-15.3041,19.79987,15.30416]},
+    {"t":1.11915, "x":1.28765, "y":0.98048, "heading":1.66915, "vx":-1.336, "vy":-0.68105, "omega":-0.69361, "ax":0.0, "ay":0.0, "alpha":3.38534, "fx":[-14.32499,17.4615,14.32499,-17.4615], "fy":[-17.4615,-14.32499,17.4615,14.32499]},
+    {"t":1.1543, "x":1.24068, "y":0.95654, "heading":1.64477, "vx":-1.336, "vy":-0.68105, "omega":-0.5746, "ax":0.0, "ay":0.0, "alpha":3.03736, "fx":[-13.23068,15.34862,13.23068,-15.34862], "fy":[-15.34863,-13.23068,15.34862,13.23068]},
+    {"t":1.18945, "x":1.19371, "y":0.9326, "heading":1.62457, "vx":-1.336, "vy":-0.68105, "omega":-0.46782, "ax":0.0, "ay":0.0, "alpha":2.70609, "fx":[-12.0615,13.43377,12.0615,-13.43377], "fy":[-13.43377,-12.0615,13.43377,12.0615]},
+    {"t":1.22461, "x":1.14675, "y":0.90866, "heading":1.60812, "vx":-1.336, "vy":-0.68105, "omega":-0.37269, "ax":0.0, "ay":0.0, "alpha":2.39043, "fx":[-10.84823,11.68989,10.84823,-11.68989], "fy":[-11.68989,-10.84823,11.68989,10.84823]},
+    {"t":1.25976, "x":1.09978, "y":0.88472, "heading":1.59502, "vx":-1.336, "vy":-0.68105, "omega":-0.28866, "ax":0.0, "ay":0.0, "alpha":2.0891, "fx":[-9.61377,10.09121,9.61377,-10.09121], "fy":[-10.09121,-9.61377,10.09121,9.61377]},
+    {"t":1.29492, "x":1.05282, "y":0.86077, "heading":1.58487, "vx":-1.336, "vy":-0.68105, "omega":-0.21522, "ax":0.0, "ay":0.0, "alpha":1.80073, "fx":[-8.3746,8.61377,8.3746,-8.61377], "fy":[-8.61377,-8.3746,8.61377,8.3746]},
+    {"t":1.33007, "x":1.00585, "y":0.83683, "heading":1.57731, "vx":-1.336, "vy":-0.68105, "omega":-0.15191, "ax":0.0, "ay":0.0, "alpha":1.52388, "fx":[-7.14198,7.2356,7.14198,-7.2356], "fy":[-7.2356,-7.14198,7.2356,7.14198]},
+    {"t":1.36523, "x":0.95888, "y":0.81289, "heading":1.57197, "vx":-1.336, "vy":-0.68105, "omega":-0.09834, "ax":0.0, "ay":0.0, "alpha":1.25699, "fx":[-5.92296,5.93685,5.92296,-5.93685], "fy":[-5.93685,-5.92296,5.93685,5.92296]},
+    {"t":1.40038, "x":0.91192, "y":0.78895, "heading":1.56851, "vx":-1.336, "vy":-0.68105, "omega":-0.05415, "ax":0.0, "ay":0.0, "alpha":0.9985, "fx":[-4.72121,4.69967,4.72121,-4.69967], "fy":[-4.69967,-4.72121,4.69967,4.72121]},
+    {"t":1.43554, "x":0.86495, "y":0.765, "heading":1.56661, "vx":-1.336, "vy":-0.68105, "omega":-0.01905, "ax":0.0, "ay":0.0, "alpha":0.74677, "fx":[-3.53766,3.50814,3.53766,-3.50814], "fy":[-3.50814,-3.53766,3.50814,3.53766]},
+    {"t":1.47069, "x":0.81798, "y":0.74106, "heading":1.56594, "vx":-1.336, "vy":-0.68105, "omega":0.0072, "ax":0.0, "ay":0.0, "alpha":0.50017, "fx":[-2.37103,2.3481,2.37103,-2.3481], "fy":[-2.3481,-2.37103,2.3481,2.37103]},
+    {"t":1.50585, "x":0.77102, "y":0.71712, "heading":1.56619, "vx":-1.336, "vy":-0.68105, "omega":0.02478, "ax":0.0, "ay":0.0, "alpha":0.25704, "fx":[-1.21815,1.20698,1.21815,-1.20698], "fy":[-1.20698,-1.21815,1.20698,1.21815]},
+    {"t":1.541, "x":0.72405, "y":0.69318, "heading":1.56706, "vx":-1.336, "vy":-0.68105, "omega":0.03382, "ax":0.0, "ay":0.0, "alpha":0.01568, "fx":[-0.07427,0.07372,0.07427,-0.07372], "fy":[-0.07372,-0.07427,0.07372,0.07427]},
+    {"t":1.57615, "x":0.67708, "y":0.66924, "heading":1.56825, "vx":-1.336, "vy":-0.68105, "omega":0.03437, "ax":0.0, "ay":0.0, "alpha":-0.22556, "fx":[1.06678,-1.06136,-1.06678,1.06136], "fy":[1.06136,1.06678,-1.06136,-1.06678]},
+    {"t":1.61131, "x":0.63012, "y":0.64529, "heading":1.56946, "vx":-1.336, "vy":-0.68105, "omega":0.02644, "ax":0.00017, "ay":0.00009, "alpha":-0.46837, "fx":[2.21538,-2.20369,-2.20961,2.20947], "fy":[2.20805,2.21397,-2.20511,-2.21102]},
+    {"t":1.64646, "x":0.58315, "y":0.62135, "heading":1.57039, "vx":-1.33599, "vy":-0.68105, "omega":0.00998, "ax":4.54388, "ay":2.31634, "alpha":-0.26554, "fx":[75.58472,73.73834,74.6825,76.49808], "fy":[39.40945,40.36451,37.15979,36.25458]},
+    {"t":1.68162, "x":0.53899, "y":0.59884, "heading":1.57074, "vx":-1.17626, "vy":-0.59962, "omega":0.00064, "ax":6.66909, "ay":3.39971, "alpha":-0.00609, "fx":[110.25209,110.22794,110.27387,110.29797], "fy":[56.23139,56.27732,56.18653,56.14064]},
+    {"t":1.71677, "x":0.50176, "y":0.57986, "heading":1.57076, "vx":-0.94181, "vy":-0.48011, "omega":0.00043, "ax":6.69002, "ay":3.41039, "alpha":-0.00386, "fx":[110.60204,110.58687,110.61615,110.63129], "fy":[56.3996,56.42888,56.37122,56.34195]},
+    {"t":1.75193, "x":0.47279, "y":0.56509, "heading":1.57078, "vx":-0.70662, "vy":-0.36022, "omega":0.00029, "ax":6.69708, "ay":3.41399, "alpha":-0.00311, "fx":[110.72013,110.70795,110.73158,110.74374], "fy":[56.45635,56.47998,56.43351,56.4099]},
+    {"t":1.78708, "x":0.45209, "y":0.55454, "heading":1.57079, "vx":-0.47119, "vy":-0.2402, "omega":0.00018, "ax":6.70063, "ay":3.41579, "alpha":-0.00273, "fx":[110.77944,110.76876,110.78955,110.80021], "fy":[56.48486,56.50564,56.46479,56.44402]},
+    {"t":1.82224, "x":0.43966, "y":0.5482, "heading":1.57079, "vx":-0.23563, "vy":-0.12012, "omega":0.00009, "ax":6.70276, "ay":3.41688, "alpha":-0.0025, "fx":[110.81511,110.80533,110.8244,110.83417], "fy":[56.502,56.52107,56.48361,56.46454]},
+    {"t":1.85739, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/rebuiltChoreo.chor b/src/main/deploy/choreo/rebuiltChoreo.chor
index a081ffec..b192f864 100644
--- a/src/main/deploy/choreo/rebuiltChoreo.chor
+++ b/src/main/deploy/choreo/rebuiltChoreo.chor
@@ -274,12 +274,12 @@
    },
    "O":{
     "x":{
-     "exp":"0.7321799635887146 m",
-     "val":0.7321799635887146
+     "exp":"0.4355199635028839 m",
+     "val":0.4355199635028839
     },
     "y":{
-     "exp":"0.7198631167411804 m",
-     "val":0.7198631167411804
+     "exp":"0.5460934042930603 m",
+     "val":0.5460934042930603
     },
     "heading":{
      "exp":"3.141592653589793 / 2 rad",
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 09cdf3ee..55941926 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -221,7 +221,7 @@ public Command climbPath(Path path, AutoRoutine routine) {
     return Commands.sequence(
         setAutoScoreReqFalse(),
         setAutoIntakeReqFalse(),
-        setAutoPreClimbReqTrue(),
+
         // Commands.parallel(
         path.getTrajectory(routine)
             .cmd()
@@ -234,6 +234,7 @@ public Command climbPath(Path path, AutoRoutine routine) {
                 path.getTrajectory(routine).done()),
         Commands.parallel(swerve.stop(), setAutoScoreReqTrue()).repeatedly().withTimeout(2.5),
         setAutoScoreReqFalse(),
+        setAutoPreClimbReqTrue(),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
             Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 281af9c9..2123902b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -651,7 +651,11 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                         shooter.runHoodCurrentZeroing(), intake.runCurrentZeroing())));
 
     new Trigger(() -> AutoAim.targetInTurretDeadzone())
-        .onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25).alongWith(operator.rumbleCmd(1, 1).withTimeout(0.25)));
+        .onTrue(
+            driver
+                .rumbleCmd(1, 1)
+                .withTimeout(0.25)
+                .alongWith(operator.rumbleCmd(1, 1).withTimeout(0.25)));
     // ---zeroing stuff---
     driver.povUp().whileTrue(shooter.currentZeroTurretAgainstForwardHardstop());
 
@@ -705,7 +709,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                     -1
                         * modifyJoystick(driver.getLeftX())
                         * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
-                shooter::getTurretPosition));  
+                shooter::getTurretPosition));
 
     new Trigger(AutoAim::targetInTurretDeadzone)
         .and(() -> Superstructure.getState().isAFeedState())
@@ -722,9 +726,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                         * modifyJoystick(driver.getLeftX())
                         * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
                 shooter::getTurretPosition,
-                 () -> Superstructure.getFeedTarget()));  
-                
-                
+                () -> Superstructure.getFeedTarget()));
 
     // create triggers for joystick disconnect alerts
     new Trigger(() -> DriverStation.isJoystickConnected(0))
diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index c47c6018..98585d0f 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -65,10 +65,7 @@ public boolean isAScoreState() {
     }
 
     public boolean isAFeedState() {
-      return this == FEED
-          || this == SPIN_UP_FEED
-          || this == SPIN_UP_FEED_FLOW
-          || this == FEED_FLOW;
+      return this == FEED || this == SPIN_UP_FEED || this == SPIN_UP_FEED_FLOW || this == FEED_FLOW;
     }
   }
 
@@ -373,8 +370,8 @@ private void addTransitions() {
     bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate());
 
     (preClimbReq.and(climbReq.negate())
-    // .and(() -> DriverStation.isTeleop())
-    )
+        // .and(() -> DriverStation.isTeleop())
+        )
         .onTrue(changeStateTo(SuperState.PRE_CLIMB));
 
     bindTransition(
diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index 2f9304c2..98a39b1e 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -243,7 +243,7 @@ public static CANcoderConfiguration getCancoderConfig() {
     CANcoderConfiguration config = new CANcoderConfiguration();
 
     config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
-    config.MagnetSensor.MagnetOffset = 0.262;
+    config.MagnetSensor.MagnetOffset = 0.26196;
     config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
 
     return config;
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 287f344a..e5ca680d 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -38,8 +38,8 @@
 import frc.robot.Robot;
 import frc.robot.Robot.RobotEdition;
 import frc.robot.Robot.RobotMode;
-import frc.robot.Superstructure.FeedTarget;
 import frc.robot.Superstructure;
+import frc.robot.Superstructure.FeedTarget;
 import frc.robot.components.camera.Camera;
 import frc.robot.components.camera.CameraIOReal;
 import frc.robot.components.camera.CameraIOSim;
@@ -737,7 +737,10 @@ public Command faceHubComp(
   }
 
   public Command faceFeedComp(
-      DoubleSupplier xVel, DoubleSupplier yVel, Supplier<Rotation2d> turretRotation, Supplier<FeedTarget> feedTargetSupplier) {
+      DoubleSupplier xVel,
+      DoubleSupplier yVel,
+      Supplier<Rotation2d> turretRotation,
+      Supplier<FeedTarget> feedTargetSupplier) {
     return driveWithHeadingSnap(
         () -> {
           Pose2d turretPose =
@@ -767,7 +770,6 @@ public Command faceFeedComp(
         yVel);
   }
 
-
   public boolean isFacingTarget(InterpolatingShotTree tree) {
     switch (Superstructure.getShotTarget()) { // ugh maybe this should be in robot.java
       case SCORE:

From d2df5ffd76beb7740105be7ec67f163f2ab6e133 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 15:45:54 -0700
Subject: [PATCH 42/54] Update at 'Sat Mar 14 15:45:54 PDT 2026'

---
 src/main/java/frc/robot/Autos.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 55941926..9275ac07 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -235,6 +235,7 @@ public Command climbPath(Path path, AutoRoutine routine) {
         Commands.parallel(swerve.stop(), setAutoScoreReqTrue()).repeatedly().withTimeout(2.5),
         setAutoScoreReqFalse(),
         setAutoPreClimbReqTrue(),
+        swerve.stop().until(() -> climber.atFullExtension()),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
             Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))

From fb86c3cf3ed82148e4dbe374c8eca854fa2bd881 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 16:46:34 -0700
Subject: [PATCH 43/54] Update at 'Sat Mar 14 16:46:34 PDT 2026'

---
 src/main/deploy/choreo/BtoD.traj |  57 ++++++++--------
 src/main/deploy/choreo/DtoS.traj | 109 ++++++++++++++++---------------
 2 files changed, 84 insertions(+), 82 deletions(-)

diff --git a/src/main/deploy/choreo/BtoD.traj b/src/main/deploy/choreo/BtoD.traj
index 5ba90945..bcf48f96 100644
--- a/src/main/deploy/choreo/BtoD.traj
+++ b/src/main/deploy/choreo/BtoD.traj
@@ -4,8 +4,8 @@
  "snapshot":{
   "waypoints":[
     {"x":3.631879568099976, "y":5.975247383117676, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.7998199462890625, "y":5.975247383117676, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":0.602399767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":1.7998199462890625, "y":5.975247383117676, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.551599767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -17,8 +17,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"3.6318795680999756 m", "val":3.631879568099976}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"D.x - 4 in", "val":0.602399767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"D.x - 6 in", "val":0.551599767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -55,7 +55,7 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.05853,1.95793],
+  "waypoints":[0.0,1.05853,1.9918],
   "samples":[
     {"t":0.0, "x":3.63188, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52604, "ay":0.0, "alpha":0.0, "fx":[-124.43129,-124.43129,-124.43129,-124.43129], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.04411, "x":3.62456, "y":5.97525, "heading":3.14159, "vx":-0.33194, "vy":0.0, "omega":0.0, "ax":-7.52462, "ay":0.0, "alpha":0.0, "fx":[-124.40791,-124.40791,-124.40791,-124.40791], "fy":[0.0,0.0,0.0,0.0]},
@@ -63,7 +63,7 @@
     {"t":0.13232, "x":3.56601, "y":5.97525, "heading":3.14159, "vx":-0.99559, "vy":0.0, "omega":0.0, "ax":-7.51893, "ay":0.0, "alpha":0.0, "fx":[-124.31375,-124.31375,-124.31375,-124.31375], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.17642, "x":3.51478, "y":5.97525, "heading":3.14159, "vx":-1.32722, "vy":0.0, "omega":0.0, "ax":-7.51173, "ay":0.0, "alpha":0.0, "fx":[-124.19469,-124.19469,-124.19469,-124.19469], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.22053, "x":3.44894, "y":5.97525, "heading":3.14159, "vx":-1.65853, "vy":0.0, "omega":0.0, "ax":-7.48958, "ay":0.0, "alpha":0.0, "fx":[-123.82854,-123.82854,-123.82854,-123.82854], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.26463, "x":3.3685, "y":5.97525, "heading":3.14159, "vx":-1.98886, "vy":0.0, "omega":0.0, "ax":-0.24223, "ay":0.0, "alpha":0.0, "fx":[-4.00486,-4.00486,-4.00486,-4.00486], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.26463, "x":3.3685, "y":5.97525, "heading":3.14159, "vx":-1.98886, "vy":0.0, "omega":0.0, "ax":-0.24223, "ay":0.0, "alpha":0.0, "fx":[-4.00483,-4.00483,-4.00483,-4.00483], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.30874, "x":3.28055, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00006,-0.00006,-0.00006,-0.00006], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.35284, "x":3.19236, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.39695, "x":3.10417, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
@@ -79,29 +79,30 @@
     {"t":0.838, "x":2.22227, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.88211, "x":2.13408, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
     {"t":0.92621, "x":2.04589, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.00008, "ay":0.0, "alpha":0.0, "fx":[0.00133,0.00133,0.00133,0.00133], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.97032, "x":1.9577, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":3.84394, "ay":0.0, "alpha":0.0, "fx":[63.55357,63.55357,63.55357,63.55357], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.97032, "x":1.9577, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":3.84397, "ay":0.0, "alpha":0.0, "fx":[63.55402,63.55402,63.55402,63.55402], "fy":[0.0,0.0,0.0,0.0]},
     {"t":1.01442, "x":1.87325, "y":5.97525, "heading":3.14159, "vx":-1.83, "vy":0.0, "omega":0.0, "ax":7.49157, "ay":0.0, "alpha":0.0, "fx":[123.8614,123.8614,123.8614,123.8614], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.05853, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49958, "vy":0.0, "omega":0.0, "ax":-0.00192, "ay":0.0, "alpha":0.0, "fx":[-0.03168,-0.03168,-0.03168,-0.03168], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.1035, "x":1.73238, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.14847, "x":1.66494, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.19344, "x":1.5975, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.23841, "x":1.53006, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.28338, "x":1.46262, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.32835, "x":1.39518, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.37332, "x":1.32774, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.41829, "x":1.2603, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.46326, "x":1.19286, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.50823, "x":1.12542, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.5532, "x":1.05798, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.59817, "x":0.99054, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.64314, "x":0.9231, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.68811, "x":0.85566, "y":5.97525, "heading":3.14159, "vx":-1.49967, "vy":0.0, "omega":0.0, "ax":0.00003, "ay":0.0, "alpha":0.0, "fx":[0.00052,0.00052,0.00052,0.00052], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.73308, "x":0.78822, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":3.27759, "ay":0.0, "alpha":0.0, "fx":[54.18983,54.18983,54.18983,54.18983], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.77805, "x":0.7241, "y":5.97525, "heading":3.14159, "vx":-1.35227, "vy":0.0, "omega":0.0, "ax":7.50347, "ay":0.0, "alpha":0.0, "fx":[124.05823,124.05823,124.05823,124.05823], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.82302, "x":0.67087, "y":5.97525, "heading":3.14159, "vx":-1.01484, "vy":0.0, "omega":0.0, "ax":7.51823, "ay":0.0, "alpha":0.0, "fx":[124.30219,124.30219,124.30219,124.30219], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.86799, "x":0.63284, "y":5.97525, "heading":3.14159, "vx":-0.67674, "vy":0.0, "omega":0.0, "ax":7.52316, "ay":0.0, "alpha":0.0, "fx":[124.38377,124.38377,124.38377,124.38377], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.91296, "x":0.61001, "y":5.97525, "heading":3.14159, "vx":-0.33843, "vy":0.0, "omega":0.0, "ax":7.52563, "ay":0.0, "alpha":0.0, "fx":[124.42461,124.42461,124.42461,124.42461], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.95793, "x":0.6024, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":1.05853, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49958, "vy":0.0, "omega":0.0, "ax":-0.0019, "ay":0.0, "alpha":0.0, "fx":[-0.03135,-0.03135,-0.03135,-0.03135], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.10297, "x":1.73317, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.14741, "x":1.66653, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.19185, "x":1.59988, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.23629, "x":1.53323, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.28073, "x":1.46659, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.32518, "x":1.39994, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.36962, "x":1.33329, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.41406, "x":1.26664, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.4585, "x":1.2, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.50294, "x":1.13335, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.54738, "x":1.0667, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.59182, "x":1.00005, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.63627, "x":0.93341, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.68071, "x":0.86676, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.72515, "x":0.80011, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.00004, "ay":0.0, "alpha":0.0, "fx":[0.00065,0.00065,0.00065,0.00065], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.76959, "x":0.73347, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":3.67539, "ay":0.0, "alpha":0.0, "fx":[60.76684,60.76684,60.76684,60.76684], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.81403, "x":0.67045, "y":5.97525, "heading":3.14159, "vx":-1.33632, "vy":0.0, "omega":0.0, "ax":7.50288, "ay":0.0, "alpha":0.0, "fx":[124.04837,124.04837,124.04837,124.04837], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.85847, "x":0.61847, "y":5.97525, "heading":3.14159, "vx":-1.00288, "vy":0.0, "omega":0.0, "ax":7.51792, "ay":0.0, "alpha":0.0, "fx":[124.2971,124.2971,124.2971,124.2971], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.90291, "x":0.58132, "y":5.97525, "heading":3.14159, "vx":-0.66877, "vy":0.0, "omega":0.0, "ax":7.52296, "ay":0.0, "alpha":0.0, "fx":[124.38035,124.38035,124.38035,124.38035], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.94736, "x":0.55903, "y":5.97525, "heading":3.14159, "vx":-0.33444, "vy":0.0, "omega":0.0, "ax":7.52548, "ay":0.0, "alpha":0.0, "fx":[124.42203,124.42203,124.42203,124.42203], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.9918, "x":0.5516, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/DtoS.traj b/src/main/deploy/choreo/DtoS.traj
index 77fb9b08..5aebd467 100644
--- a/src/main/deploy/choreo/DtoS.traj
+++ b/src/main/deploy/choreo/DtoS.traj
@@ -3,7 +3,7 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":0.703999767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.551599767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":2.1980347633361816, "y":5.884655952453613, "heading":3.141666845264474, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":2.4044814109802246, "y":2.0, "heading":-3.141507624207323, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
@@ -15,7 +15,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"D.x", "val":0.703999767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"D.x - 6 in", "val":0.551599767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"2.1980347633361816 m", "val":2.1980347633361816}, "y":{"exp":"5.884655952453613 m", "val":5.884655952453613}, "heading":{"exp":"3.1416668452644734 rad", "val":3.141666845264474}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"SC.x", "val":2.4044814109802246}, "y":{"exp":"SC.y", "val":2.0}, "heading":{"exp":"SC.heading", "val":-3.141507624207323}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
@@ -53,59 +53,60 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,0.96945,3.2719],
+  "waypoints":[0.0,1.0542,3.35646],
   "samples":[
-    {"t":0.0, "x":0.704, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.52414, "ay":0.2021, "alpha":0.0, "fx":[124.39987,124.39987,124.39987,124.39987], "fy":[3.34146,3.34148,3.34148,3.34146]},
-    {"t":0.05386, "x":0.71491, "y":5.97554, "heading":3.14159, "vx":0.40524, "vy":0.01088, "omega":0.0, "ax":7.52207, "ay":0.20205, "alpha":0.0, "fx":[124.36574,124.36574,124.36574,124.36574], "fy":[3.34054,3.34056,3.34056,3.34054]},
-    {"t":0.10772, "x":0.74765, "y":5.97642, "heading":-3.14159, "vx":0.81036, "vy":0.02177, "omega":0.0, "ax":7.51795, "ay":0.20194, "alpha":0.0, "fx":[124.29755,124.29755,124.29755,124.29755], "fy":[3.3387,3.33874,3.33874,3.33871]},
-    {"t":0.16157, "x":0.8022, "y":5.97788, "heading":-3.14159, "vx":1.21526, "vy":0.03264, "omega":0.0, "ax":7.50561, "ay":0.20161, "alpha":0.0, "fx":[124.09355,124.09355,124.09355,124.09355], "fy":[3.33321,3.33327,3.33327,3.33322]},
-    {"t":0.21543, "x":0.87853, "y":5.97994, "heading":-3.14159, "vx":1.6195, "vy":0.0435, "omega":0.0, "ax":3.33308, "ay":0.08953, "alpha":0.00049, "fx":[55.10908,55.10903,55.10534,55.10539], "fy":[1.47746,1.48293,1.48298,1.4775]},
-    {"t":0.26929, "x":0.97059, "y":5.98241, "heading":-3.14159, "vx":1.79902, "vy":0.04832, "omega":0.00003, "ax":0.00002, "ay":0.0, "alpha":0.00059, "fx":[0.00317,0.00317,-0.00242,-0.00242], "fy":[-0.00278,0.0028,0.0028,-0.00278]},
-    {"t":0.32315, "x":1.06748, "y":5.98501, "heading":-3.14159, "vx":1.79902, "vy":0.04832, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00048, "fx":[0.00224,0.00224,-0.00224,-0.00224], "fy":[-0.00224,0.00224,0.00224,-0.00224]},
-    {"t":0.37701, "x":1.16437, "y":5.98761, "heading":-3.14159, "vx":1.79902, "vy":0.04832, "omega":0.00008, "ax":0.0, "ay":0.0, "alpha":0.00037, "fx":[0.00174,0.00174,-0.00174,-0.00174], "fy":[-0.00174,0.00174,0.00174,-0.00174]},
-    {"t":0.43087, "x":1.26127, "y":5.99022, "heading":-3.14158, "vx":1.79902, "vy":0.04832, "omega":0.0001, "ax":0.0, "ay":0.0, "alpha":0.00027, "fx":[0.00128,0.00128,-0.00128,-0.00128], "fy":[-0.00128,0.00128,0.00128,-0.00128]},
-    {"t":0.48472, "x":1.35816, "y":5.99282, "heading":-3.14158, "vx":1.79902, "vy":0.04832, "omega":0.00012, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[0.00085,0.00085,-0.00085,-0.00085], "fy":[-0.00086,0.00085,0.00085,-0.00086]},
-    {"t":0.53858, "x":1.45505, "y":5.99542, "heading":-3.14157, "vx":1.79902, "vy":0.04832, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00045,0.00045,-0.00044,-0.00044], "fy":[-0.00049,0.00039,0.00039,-0.00049]},
-    {"t":0.59244, "x":1.55194, "y":5.99802, "heading":-3.14156, "vx":1.79902, "vy":0.04832, "omega":0.00013, "ax":0.0, "ay":-0.00011, "alpha":0.00001, "fx":[0.00009,0.00009,0.0,0.0], "fy":[-0.00187,-0.00178,-0.00178,-0.00187]},
-    {"t":0.6463, "x":1.64883, "y":6.00063, "heading":-3.14156, "vx":1.79902, "vy":0.04832, "omega":0.00013, "ax":0.00011, "ay":-0.004, "alpha":-0.00007, "fx":[0.00142,0.00142,0.00213,0.00213], "fy":[-0.06576,-0.06647,-0.06647,-0.06576]},
-    {"t":0.70016, "x":1.74572, "y":6.00322, "heading":-3.14155, "vx":1.79902, "vy":0.0481, "omega":0.00013, "ax":0.00355, "ay":-0.1447, "alpha":-0.00016, "fx":[0.05799,0.05799,0.05951,0.05951], "fy":[-2.3917,-2.39322,-2.39322,-2.3917]},
-    {"t":0.75401, "x":1.84262, "y":6.0056, "heading":-3.14154, "vx":1.79921, "vy":0.04031, "omega":0.00012, "ax":-0.13728, "ay":-3.86361, "alpha":-0.00015, "fx":[-2.2706,-2.27057,-2.26883,-2.26885], "fy":[-63.87821,-63.87923,-63.87925,-63.87823]},
-    {"t":0.80787, "x":1.93933, "y":6.00217, "heading":-3.14154, "vx":1.79182, "vy":-0.16778, "omega":0.00011, "ax":-1.48279, "ay":-7.18622, "alpha":-0.00001, "fx":[-24.51567,-24.51564,-24.51548,-24.51551], "fy":[-118.81287,-118.81288,-118.81291,-118.8129]},
-    {"t":0.86173, "x":2.03368, "y":5.98271, "heading":-3.14153, "vx":1.71196, "vy":-0.55482, "omega":0.00011, "ax":-3.08961, "ay":-6.81149, "alpha":0.0, "fx":[-51.08197,-51.08195,-51.0819,-51.08192], "fy":[-112.6173,-112.61731,-112.61733,-112.61732]},
-    {"t":0.91559, "x":2.1214, "y":5.94295, "heading":-3.14152, "vx":1.54556, "vy":-0.92167, "omega":0.00011, "ax":-4.55565, "ay":-5.96827, "alpha":0.0, "fx":[-75.32057,-75.32056,-75.32054,-75.32055], "fy":[-98.67609,-98.6761,-98.67611,-98.6761]},
-    {"t":0.96945, "x":2.19803, "y":5.88466, "heading":-3.14152, "vx":1.3002, "vy":-1.24311, "omega":0.00011, "ax":-5.9411, "ay":-4.59733, "alpha":0.0, "fx":[-98.22677,-98.22675,-98.22674,-98.22676], "fy":[-76.00968,-76.0097,-76.00971,-76.00969]},
-    {"t":1.0414, "x":2.27621, "y":5.78331, "heading":-3.14151, "vx":0.87273, "vy":-1.5739, "omega":0.00011, "ax":-6.99114, "ay":-2.60535, "alpha":-0.00001, "fx":[-115.58758,-115.58755,-115.58754,-115.58757], "fy":[-43.07542,-43.07551,-43.07554,-43.07545]},
-    {"t":1.11335, "x":2.32091, "y":5.66332, "heading":-3.1415, "vx":0.36971, "vy":-1.76135, "omega":0.00011, "ax":-4.56478, "ay":-0.52704, "alpha":-0.0002, "fx":[-75.4722,-75.47204,-75.47083,-75.47099], "fy":[-8.71235,-8.71495,-8.71511,-8.71252]},
-    {"t":1.1853, "x":2.33569, "y":5.53523, "heading":-3.14149, "vx":0.04126, "vy":-1.79928, "omega":0.0001, "ax":-0.08743, "ay":-0.00185, "alpha":-0.00036, "fx":[-1.4472,-1.4472,-1.4438,-1.4438], "fy":[-0.02894,-0.03234,-0.03234,-0.02894]},
-    {"t":1.25725, "x":2.33843, "y":5.40576, "heading":-3.14149, "vx":0.03497, "vy":-1.79941, "omega":0.00007, "ax":-0.00105, "ay":-0.00002, "alpha":-0.00029, "fx":[-0.01874,-0.01874,-0.01596,-0.01596], "fy":[0.00105,-0.00172,-0.00172,0.00105]},
-    {"t":1.32921, "x":2.34095, "y":5.27629, "heading":-3.14148, "vx":0.0349, "vy":-1.79941, "omega":0.00005, "ax":-0.00001, "ay":0.0, "alpha":-0.00024, "fx":[-0.00134,-0.00134,0.00092,0.00092], "fy":[0.00113,-0.00114,-0.00114,0.00113]},
-    {"t":1.40116, "x":2.34346, "y":5.14682, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[-0.00092,-0.00092,0.00092,0.00092], "fy":[0.00092,-0.00092,-0.00092,0.00092]},
-    {"t":1.47311, "x":2.34597, "y":5.01735, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[-0.00075,-0.00075,0.00075,0.00075], "fy":[0.00075,-0.00075,-0.00075,0.00075]},
-    {"t":1.54506, "x":2.34848, "y":4.88788, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00013, "fx":[-0.00061,-0.00061,0.00061,0.00061], "fy":[0.00061,-0.00061,-0.00061,0.00061]},
-    {"t":1.61701, "x":2.35099, "y":4.75841, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00049,-0.00049,0.00049,0.00049], "fy":[0.00049,-0.00049,-0.00049,0.00049]},
-    {"t":1.68896, "x":2.3535, "y":4.62894, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.0004,-0.0004,0.0004,0.0004], "fy":[0.0004,-0.0004,-0.0004,0.0004]},
-    {"t":1.76092, "x":2.35601, "y":4.49947, "heading":-3.14147, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00032,-0.00032,0.00032,0.00032], "fy":[0.00032,-0.00032,-0.00032,0.00032]},
-    {"t":1.83287, "x":2.35852, "y":4.37, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.00025,-0.00025,-0.00025,0.00025]},
-    {"t":1.90482, "x":2.36103, "y":4.24053, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[-0.0002,-0.0002,0.0002,0.0002], "fy":[0.0002,-0.0002,-0.0002,0.0002]},
-    {"t":1.97677, "x":2.36354, "y":4.11106, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00015,-0.00015,0.00015,0.00015], "fy":[0.00015,-0.00015,-0.00015,0.00015]},
-    {"t":2.04872, "x":2.36605, "y":3.98159, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00011,-0.00011,0.00011,0.00011], "fy":[0.00011,-0.00011,-0.00011,0.00011]},
-    {"t":2.12067, "x":2.36856, "y":3.85211, "heading":-3.14148, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00007,-0.00007,0.00007,0.00007], "fy":[0.00007,-0.00007,-0.00007,0.00007]},
-    {"t":2.19262, "x":2.37108, "y":3.72264, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,-0.00004,0.00004,0.00004], "fy":[0.00004,-0.00004,-0.00004,0.00004]},
-    {"t":2.26458, "x":2.37359, "y":3.59317, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,0.00001,0.00001], "fy":[0.00001,-0.00001,-0.00001,0.00001]},
-    {"t":2.33653, "x":2.3761, "y":3.4637, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,-0.00002,-0.00002], "fy":[-0.00002,0.00002,0.00002,-0.00002]},
-    {"t":2.40848, "x":2.37861, "y":3.33423, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00005,0.00005,-0.00005,-0.00005], "fy":[-0.00005,0.00005,0.00005,-0.00005]},
-    {"t":2.48043, "x":2.38112, "y":3.20476, "heading":-3.14149, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[0.00009,0.00009,-0.00009,-0.00009], "fy":[-0.00009,0.00009,0.00009,-0.00009]},
-    {"t":2.55238, "x":2.38363, "y":3.07529, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00013,0.00013,-0.00013,-0.00013], "fy":[-0.00013,0.00013,0.00013,-0.00013]},
-    {"t":2.62433, "x":2.38614, "y":2.94582, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.00017,0.00017,-0.00017,-0.00017], "fy":[-0.00017,0.00017,0.00017,-0.00017]},
-    {"t":2.69629, "x":2.38865, "y":2.81635, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[0.00022,0.00022,-0.00022,-0.00022], "fy":[-0.00022,0.00022,0.00022,-0.00022]},
-    {"t":2.76824, "x":2.39116, "y":2.68688, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00028,0.00028,-0.00028,-0.00028], "fy":[-0.00028,0.00028,0.00028,-0.00028]},
-    {"t":2.84019, "x":2.39367, "y":2.55741, "heading":-3.1415, "vx":0.03489, "vy":-1.79941, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[0.00035,0.00035,-0.00035,-0.00035], "fy":[-0.00035,0.00035,0.00035,-0.00035]},
-    {"t":2.91214, "x":2.39618, "y":2.42794, "heading":-3.14151, "vx":0.03489, "vy":-1.79941, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00043,0.00043,-0.00043,-0.00043], "fy":[-0.00035,0.00051,0.00051,-0.00035]},
-    {"t":2.98409, "x":2.39869, "y":2.29847, "heading":-3.14151, "vx":0.03489, "vy":-1.79941, "omega":-0.00001, "ax":-0.0473, "ay":2.43926, "alpha":0.00009, "fx":[-0.78161,-0.7816,-0.78256,-0.78256], "fy":[40.32899,40.32976,40.32976,40.32899]},
-    {"t":3.05604, "x":2.40108, "y":2.17531, "heading":-3.14151, "vx":0.03149, "vy":-1.6239, "omega":0.0, "ax":-0.14579, "ay":7.5177, "alpha":0.0, "fx":[-2.41035,-2.41035,-2.41035,-2.41035], "fy":[124.29345,124.29345,124.29345,124.29345]},
-    {"t":3.128, "x":2.40297, "y":2.07793, "heading":-3.14151, "vx":0.021, "vy":-1.08299, "omega":0.0, "ax":-0.14592, "ay":7.52466, "alpha":0.0, "fx":[-2.41258,-2.41258,-2.41258,-2.41258], "fy":[124.40858,124.40858,124.40858,124.40858]},
-    {"t":3.19995, "x":2.4041, "y":2.01948, "heading":-3.14151, "vx":0.0105, "vy":-0.54158, "omega":0.0, "ax":-0.14597, "ay":7.52699, "alpha":0.0, "fx":[-2.41332,-2.41332,-2.41333,-2.41333], "fy":[124.44699,124.44699,124.44699,124.44699]},
-    {"t":3.2719, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":0.5516, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.52492, "ay":0.18376, "alpha":0.0, "fx":[124.41283,124.41283,124.41283,124.41283], "fy":[3.03817,3.03819,3.03819,3.03817]},
+    {"t":0.05548, "x":0.56318, "y":5.97553, "heading":3.14159, "vx":0.41751, "vy":0.0102, "omega":0.0, "ax":7.52296, "ay":0.18371, "alpha":0.0, "fx":[124.38041,124.38041,124.38041,124.38041], "fy":[3.03738,3.0374,3.0374,3.03738]},
+    {"t":0.11097, "x":0.59793, "y":5.97638, "heading":-3.14159, "vx":0.83492, "vy":0.02039, "omega":0.0, "ax":7.51904, "ay":0.18362, "alpha":0.0, "fx":[124.31558,124.31558,124.31558,124.31558], "fy":[3.03579,3.03582,3.03582,3.03579]},
+    {"t":0.16645, "x":0.65583, "y":5.97779, "heading":-3.14159, "vx":1.2521, "vy":0.03058, "omega":0.0, "ax":7.50729, "ay":0.18333, "alpha":0.0, "fx":[124.12135,124.12135,124.12135,124.12135], "fy":[3.03104,3.03108,3.03108,3.03104]},
+    {"t":0.22194, "x":0.73685, "y":5.97977, "heading":-3.14159, "vx":1.66864, "vy":0.04075, "omega":0.0, "ax":2.35197, "ay":0.05744, "alpha":0.00048, "fx":[38.88825,38.88823,38.88411,38.88413], "fy":[0.94709,0.9521,0.95212,0.94711]},
+    {"t":0.27742, "x":0.83306, "y":5.98212, "heading":-3.14159, "vx":1.79914, "vy":0.04394, "omega":0.00003, "ax":0.00001, "ay":0.0, "alpha":0.00048, "fx":[0.00249,0.00249,-0.00207,-0.00207], "fy":[-0.00228,0.00229,0.00229,-0.00228]},
+    {"t":0.3329, "x":0.93288, "y":5.98456, "heading":-3.14159, "vx":1.79914, "vy":0.04394, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00039, "fx":[0.00184,0.00184,-0.00184,-0.00184], "fy":[-0.00184,0.00184,0.00184,-0.00184]},
+    {"t":0.38839, "x":1.0327, "y":5.987, "heading":-3.14159, "vx":1.79914, "vy":0.04394, "omega":0.00008, "ax":0.0, "ay":0.0, "alpha":0.00031, "fx":[0.00145,0.00145,-0.00145,-0.00145], "fy":[-0.00145,0.00145,0.00145,-0.00145]},
+    {"t":0.44387, "x":1.13253, "y":5.98943, "heading":-3.14158, "vx":1.79914, "vy":0.04394, "omega":0.00009, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00109,0.00109,-0.00109,-0.00109], "fy":[-0.00109,0.00109,0.00109,-0.00109]},
+    {"t":0.49936, "x":1.23235, "y":5.99187, "heading":-3.14158, "vx":1.79914, "vy":0.04394, "omega":0.00011, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[0.00076,0.00076,-0.00076,-0.00076], "fy":[-0.00076,0.00076,0.00076,-0.00076]},
+    {"t":0.55484, "x":1.33217, "y":5.99431, "heading":-3.14157, "vx":1.79914, "vy":0.04394, "omega":0.00011, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00045,0.00045,-0.00045,-0.00045], "fy":[-0.00045,0.00045,0.00045,-0.00045]},
+    {"t":0.61032, "x":1.432, "y":5.99675, "heading":-3.14157, "vx":1.79914, "vy":0.04394, "omega":0.00012, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00014,0.00014,-0.00014,-0.00014], "fy":[-0.00017,0.00012,0.00012,-0.00017]},
+    {"t":0.66581, "x":1.53182, "y":5.99918, "heading":-3.14156, "vx":1.79914, "vy":0.04394, "omega":0.00012, "ax":0.0, "ay":-0.00006, "alpha":-0.00003, "fx":[-0.00013,-0.00013,0.00018,0.00018], "fy":[-0.00091,-0.00122,-0.00122,-0.00091]},
+    {"t":0.72129, "x":1.63164, "y":6.00162, "heading":-3.14155, "vx":1.79914, "vy":0.04393, "omega":0.00012, "ax":0.00006, "ay":-0.00252, "alpha":-0.0001, "fx":[0.00056,0.00056,0.00147,0.00147], "fy":[-0.04113,-0.04205,-0.04205,-0.04113]},
+    {"t":0.77678, "x":1.73147, "y":6.00406, "heading":-3.14155, "vx":1.79914, "vy":0.04379, "omega":0.00011, "ax":0.00225, "ay":-0.09843, "alpha":-0.00016, "fx":[0.03635,0.03635,0.03789,0.03789], "fy":[-1.62654,-1.62808,-1.62808,-1.62654]},
+    {"t":0.83226, "x":1.83129, "y":6.00633, "heading":-3.14154, "vx":1.79927, "vy":0.03833, "omega":0.00011, "ax":-0.08776, "ay":-3.17172, "alpha":-0.00016, "fx":[-1.45186,-1.45185,-1.45003,-1.45005], "fy":[-52.4388,-52.44007,-52.44008,-52.43881]},
+    {"t":0.88775, "x":1.93099, "y":6.00358, "heading":-3.14153, "vx":1.7944, "vy":-0.13765, "omega":0.0001, "ax":-1.37796, "ay":-7.18047, "alpha":-0.00001, "fx":[-22.78244,-22.78241,-22.78225,-22.78228], "fy":[-118.71785,-118.71786,-118.71789,-118.71788]},
+    {"t":0.94323, "x":2.02843, "y":5.98489, "heading":-3.14153, "vx":1.71794, "vy":-0.53605, "omega":0.0001, "ax":-3.03741, "ay":-6.83361, "alpha":0.0, "fx":[-50.21884,-50.21882,-50.21877,-50.21879], "fy":[-112.98309,-112.9831,-112.98312,-112.98311]},
+    {"t":0.99871, "x":2.11907, "y":5.94463, "heading":-3.14152, "vx":1.54941, "vy":-0.91521, "omega":0.0001, "ax":-4.55098, "ay":-5.97224, "alpha":0.0, "fx":[-75.2434,-75.24339,-75.24337,-75.24338], "fy":[-98.74161,-98.74163,-98.74164,-98.74163]},
+    {"t":1.0542, "x":2.19803, "y":5.88466, "heading":-3.14152, "vx":1.29691, "vy":-1.24657, "omega":0.0001, "ax":-5.95321, "ay":-4.58135, "alpha":0.0, "fx":[-98.42699,-98.42698,-98.42697,-98.42698], "fy":[-75.74549,-75.74551,-75.74552,-75.7455]},
+    {"t":1.12614, "x":2.27593, "y":5.78311, "heading":-3.14151, "vx":0.8686, "vy":-1.57618, "omega":0.0001, "ax":-6.99683, "ay":-2.58686, "alpha":0.0, "fx":[-115.68169,-115.68166,-115.68165,-115.68168], "fy":[-42.76971,-42.76978,-42.7698,-42.76973]},
+    {"t":1.19809, "x":2.32032, "y":5.66302, "heading":-3.1415, "vx":0.36521, "vy":-1.76229, "omega":0.00009, "ax":-4.49998, "ay":-0.51398, "alpha":-0.00017, "fx":[-74.40073,-74.4006,-74.39954,-74.39967], "fy":[-8.49664,-8.49885,-8.49898,-8.49677]},
+    {"t":1.27003, "x":2.33495, "y":5.5349, "heading":-3.1415, "vx":0.04145, "vy":-1.79927, "omega":0.00008, "ax":-0.08478, "ay":-0.00181, "alpha":-0.0003, "fx":[-1.40307,-1.40307,-1.40023,-1.40023], "fy":[-0.02851,-0.03135,-0.03135,-0.02851]},
+    {"t":1.34198, "x":2.33771, "y":5.40545, "heading":-3.14149, "vx":0.03535, "vy":-1.7994, "omega":0.00006, "ax":-0.00102, "ay":-0.00002, "alpha":-0.00025, "fx":[-0.01799,-0.01799,-0.01567,-0.01567], "fy":[0.00083,-0.00149,-0.00149,0.00083]},
+    {"t":1.41393, "x":2.34025, "y":5.27599, "heading":-3.14149, "vx":0.03528, "vy":-1.7994, "omega":0.00004, "ax":-0.00001, "ay":0.0, "alpha":-0.0002, "fx":[-0.00115,-0.00115,0.00074,0.00074], "fy":[0.00094,-0.00095,-0.00095,0.00094]},
+    {"t":1.48587, "x":2.34279, "y":5.14653, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[-0.00077,-0.00077,0.00077,0.00077], "fy":[0.00077,-0.00077,-0.00077,0.00077]},
+    {"t":1.55782, "x":2.34533, "y":5.01707, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00013, "fx":[-0.00063,-0.00063,0.00063,0.00063], "fy":[0.00063,-0.00063,-0.00063,0.00063]},
+    {"t":1.62976, "x":2.34787, "y":4.88761, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[-0.00051,-0.00051,0.00051,0.00051], "fy":[0.00051,-0.00051,-0.00051,0.00051]},
+    {"t":1.70171, "x":2.3504, "y":4.75815, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.00041,-0.00041,0.00041,0.00041], "fy":[0.00041,-0.00041,-0.00041,0.00041]},
+    {"t":1.77365, "x":2.35294, "y":4.62869, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00033,-0.00033,0.00033,0.00033], "fy":[0.00033,-0.00033,-0.00033,0.00033]},
+    {"t":1.8456, "x":2.35548, "y":4.49923, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[-0.00027,-0.00027,0.00027,0.00027], "fy":[0.00027,-0.00027,-0.00027,0.00027]},
+    {"t":1.91755, "x":2.35802, "y":4.36977, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00021,-0.00021,0.00021,0.00021], "fy":[0.00021,-0.00021,-0.00021,0.00021]},
+    {"t":1.98949, "x":2.36056, "y":4.24031, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[-0.00017,-0.00017,0.00017,0.00017], "fy":[0.00017,-0.00017,-0.00017,0.00017]},
+    {"t":2.06144, "x":2.36309, "y":4.11085, "heading":-3.14148, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.00013,-0.00013,-0.00013,0.00013]},
+    {"t":2.13338, "x":2.36563, "y":3.98139, "heading":-3.14149, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00009,-0.00009,0.00009,0.00009], "fy":[0.00009,-0.00009,-0.00009,0.00009]},
+    {"t":2.20533, "x":2.36817, "y":3.85193, "heading":-3.14149, "vx":0.03528, "vy":-1.7994, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00006,-0.00006,0.00006,0.00006], "fy":[0.00006,-0.00006,-0.00006,0.00006]},
+    {"t":2.27727, "x":2.37071, "y":3.72247, "heading":-3.14149, "vx":0.03528, "vy":-1.7994, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,-0.00004,0.00004,0.00004], "fy":[0.00004,-0.00004,-0.00004,0.00004]},
+    {"t":2.34922, "x":2.37325, "y":3.59302, "heading":-3.14149, "vx":0.03528, "vy":-1.7994, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,0.00001,0.00001], "fy":[0.00001,-0.00001,-0.00001,0.00001]},
+    {"t":2.42117, "x":2.37579, "y":3.46356, "heading":-3.14149, "vx":0.03528, "vy":-1.7994, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,-0.00002,-0.00002], "fy":[-0.00002,0.00002,0.00002,-0.00002]},
+    {"t":2.49311, "x":2.37832, "y":3.3341, "heading":-3.1415, "vx":0.03528, "vy":-1.7994, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00004,0.00004,-0.00004,-0.00004], "fy":[-0.00004,0.00004,0.00004,-0.00004]},
+    {"t":2.56506, "x":2.38086, "y":3.20464, "heading":-3.1415, "vx":0.03528, "vy":-1.7994, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[-0.00007,0.00007,0.00007,-0.00007]},
+    {"t":2.637, "x":2.3834, "y":3.07518, "heading":-3.1415, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[0.0001,0.0001,-0.0001,-0.0001], "fy":[-0.0001,0.0001,0.0001,-0.0001]},
+    {"t":2.70895, "x":2.38594, "y":2.94572, "heading":-3.1415, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00014,0.00014,-0.00014,-0.00014], "fy":[-0.00014,0.00014,0.00014,-0.00014]},
+    {"t":2.78089, "x":2.38848, "y":2.81626, "heading":-3.1415, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.00018,0.00018,-0.00018,-0.00018], "fy":[-0.00018,0.00018,0.00018,-0.00018]},
+    {"t":2.85284, "x":2.39102, "y":2.6868, "heading":-3.1415, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[0.00022,0.00022,-0.00022,-0.00022], "fy":[-0.00022,0.00022,0.00022,-0.00022]},
+    {"t":2.92478, "x":2.39355, "y":2.55734, "heading":-3.14151, "vx":0.03528, "vy":-1.7994, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00028,0.00028,-0.00028,-0.00028], "fy":[-0.00028,0.00028,0.00028,-0.00028]},
+    {"t":2.99673, "x":2.39609, "y":2.42788, "heading":-3.14151, "vx":0.03528, "vy":-1.7994, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[0.00035,0.00035,-0.00035,-0.00035], "fy":[-0.00027,0.00043,0.00043,-0.00027]},
+    {"t":3.06868, "x":2.39863, "y":2.29842, "heading":-3.14151, "vx":0.03528, "vy":-1.7994, "omega":-0.00001, "ax":-0.04787, "ay":2.44133, "alpha":0.00007, "fx":[-0.79101,-0.791,-0.79178,-0.79178], "fy":[40.36324,40.36387,40.36387,40.36324]},
+    {"t":3.14062, "x":2.40104, "y":2.17528, "heading":-3.14151, "vx":0.03184, "vy":-1.62376, "omega":0.0, "ax":-0.1474, "ay":7.51767, "alpha":0.0, "fx":[-2.43696,-2.43696,-2.43696,-2.43696], "fy":[124.29289,124.29289,124.29289,124.29289]},
+    {"t":3.21257, "x":2.40295, "y":2.07792, "heading":-3.14151, "vx":0.02123, "vy":-1.0829, "omega":0.0, "ax":-0.14753, "ay":7.52463, "alpha":0.0, "fx":[-2.43922,-2.43922,-2.43922,-2.43922], "fy":[124.40804,124.40804,124.40804,124.40804]},
+    {"t":3.28451, "x":2.4041, "y":2.01948, "heading":-3.14151, "vx":0.01062, "vy":-0.54153, "omega":0.0, "ax":-0.14758, "ay":7.52695, "alpha":0.0, "fx":[-2.43997,-2.43997,-2.43997,-2.43997], "fy":[124.44646,124.44646,124.44646,124.44646]},
+    {"t":3.35646, "x":2.40448, "y":2.0, "heading":-3.14151, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]

From 6c24189872b8833448057a2bcb6eb1951ab497e3 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 17:46:09 -0700
Subject: [PATCH 44/54] Update at 'Sat Mar 14 17:46:09 PDT 2026'

---
 src/main/deploy/choreo/BtoD.traj   | 118 ++++++++++++++++-------------
 src/main/java/frc/robot/Autos.java |   8 +-
 2 files changed, 73 insertions(+), 53 deletions(-)

diff --git a/src/main/deploy/choreo/BtoD.traj b/src/main/deploy/choreo/BtoD.traj
index bcf48f96..1ee6dd9d 100644
--- a/src/main/deploy/choreo/BtoD.traj
+++ b/src/main/deploy/choreo/BtoD.traj
@@ -3,27 +3,27 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":3.631879568099976, "y":5.975247383117676, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.631879568099976, "y":5.975247383117676, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":1.7998199462890625, "y":5.975247383117676, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":0.551599767303467, "y":5.975247383117676, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
-    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true},
     {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}],
   "targetDt":0.05
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"3.6318795680999756 m", "val":3.631879568099976}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"3.6318795680999756 m", "val":3.631879568099976}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"D.x - 6 in", "val":0.551599767303467}, "y":{"exp":"D.y", "val":5.975247383117676}, "heading":{"exp":"D.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
-    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.0}}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true},
     {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slower", "val":1.5}}}, "enabled":true}],
   "targetDt":{
    "exp":"0.05 s",
@@ -55,54 +55,70 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.05853,1.9918],
+  "waypoints":[0.0,1.3216,2.25488],
   "samples":[
-    {"t":0.0, "x":3.63188, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52604, "ay":0.0, "alpha":0.0, "fx":[-124.43129,-124.43129,-124.43129,-124.43129], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.04411, "x":3.62456, "y":5.97525, "heading":3.14159, "vx":-0.33194, "vy":0.0, "omega":0.0, "ax":-7.52462, "ay":0.0, "alpha":0.0, "fx":[-124.40791,-124.40791,-124.40791,-124.40791], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.08821, "x":3.6026, "y":5.97525, "heading":3.14159, "vx":-0.66381, "vy":0.0, "omega":0.0, "ax":-7.52249, "ay":0.0, "alpha":0.0, "fx":[-124.37271,-124.37271,-124.37271,-124.37271], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.13232, "x":3.56601, "y":5.97525, "heading":3.14159, "vx":-0.99559, "vy":0.0, "omega":0.0, "ax":-7.51893, "ay":0.0, "alpha":0.0, "fx":[-124.31375,-124.31375,-124.31375,-124.31375], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.17642, "x":3.51478, "y":5.97525, "heading":3.14159, "vx":-1.32722, "vy":0.0, "omega":0.0, "ax":-7.51173, "ay":0.0, "alpha":0.0, "fx":[-124.19469,-124.19469,-124.19469,-124.19469], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.22053, "x":3.44894, "y":5.97525, "heading":3.14159, "vx":-1.65853, "vy":0.0, "omega":0.0, "ax":-7.48958, "ay":0.0, "alpha":0.0, "fx":[-123.82854,-123.82854,-123.82854,-123.82854], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.26463, "x":3.3685, "y":5.97525, "heading":3.14159, "vx":-1.98886, "vy":0.0, "omega":0.0, "ax":-0.24223, "ay":0.0, "alpha":0.0, "fx":[-4.00483,-4.00483,-4.00483,-4.00483], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.30874, "x":3.28055, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00006,-0.00006,-0.00006,-0.00006], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.35284, "x":3.19236, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.39695, "x":3.10417, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.44105, "x":3.01598, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.48516, "x":2.92779, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.52926, "x":2.8396, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.57337, "x":2.75141, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.61747, "x":2.66322, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.66158, "x":2.57503, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.70568, "x":2.48684, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.74979, "x":2.39865, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.79389, "x":2.31046, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.838, "x":2.22227, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.88211, "x":2.13408, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.92621, "x":2.04589, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":0.00008, "ay":0.0, "alpha":0.0, "fx":[0.00133,0.00133,0.00133,0.00133], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.97032, "x":1.9577, "y":5.97525, "heading":3.14159, "vx":-1.99954, "vy":0.0, "omega":0.0, "ax":3.84397, "ay":0.0, "alpha":0.0, "fx":[63.55402,63.55402,63.55402,63.55402], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.01442, "x":1.87325, "y":5.97525, "heading":3.14159, "vx":-1.83, "vy":0.0, "omega":0.0, "ax":7.49157, "ay":0.0, "alpha":0.0, "fx":[123.8614,123.8614,123.8614,123.8614], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.05853, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49958, "vy":0.0, "omega":0.0, "ax":-0.0019, "ay":0.0, "alpha":0.0, "fx":[-0.03135,-0.03135,-0.03135,-0.03135], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.10297, "x":1.73317, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.14741, "x":1.66653, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.19185, "x":1.59988, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.23629, "x":1.53323, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.28073, "x":1.46659, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.32518, "x":1.39994, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.36962, "x":1.33329, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.41406, "x":1.26664, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.4585, "x":1.2, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.50294, "x":1.13335, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.54738, "x":1.0667, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.59182, "x":1.00005, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.63627, "x":0.93341, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.68071, "x":0.86676, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.72515, "x":0.80011, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":0.00004, "ay":0.0, "alpha":0.0, "fx":[0.00065,0.00065,0.00065,0.00065], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.76959, "x":0.73347, "y":5.97525, "heading":3.14159, "vx":-1.49966, "vy":0.0, "omega":0.0, "ax":3.67539, "ay":0.0, "alpha":0.0, "fx":[60.76684,60.76684,60.76684,60.76684], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.81403, "x":0.67045, "y":5.97525, "heading":3.14159, "vx":-1.33632, "vy":0.0, "omega":0.0, "ax":7.50288, "ay":0.0, "alpha":0.0, "fx":[124.04837,124.04837,124.04837,124.04837], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.85847, "x":0.61847, "y":5.97525, "heading":3.14159, "vx":-1.00288, "vy":0.0, "omega":0.0, "ax":7.51792, "ay":0.0, "alpha":0.0, "fx":[124.2971,124.2971,124.2971,124.2971], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.90291, "x":0.58132, "y":5.97525, "heading":3.14159, "vx":-0.66877, "vy":0.0, "omega":0.0, "ax":7.52296, "ay":0.0, "alpha":0.0, "fx":[124.38035,124.38035,124.38035,124.38035], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.94736, "x":0.55903, "y":5.97525, "heading":3.14159, "vx":-0.33444, "vy":0.0, "omega":0.0, "ax":7.52548, "ay":0.0, "alpha":0.0, "fx":[124.42203,124.42203,124.42203,124.42203], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.9918, "x":0.5516, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":3.63188, "y":5.97525, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.52372, "ay":0.0, "alpha":0.01418, "fx":[-124.39323,-124.3929,-124.3929,-124.39323], "fy":[-0.1335,-0.13379,0.13379,0.1335]},
+    {"t":0.03304, "x":3.62777, "y":5.97525, "heading":1.5708, "vx":-0.24858, "vy":0.0, "omega":0.00047, "ax":-7.52184, "ay":0.0, "alpha":0.01602, "fx":[-124.3622,-124.36175,-124.36175,-124.3622], "fy":[-0.15071,-0.15108,0.15108,0.15071]},
+    {"t":0.06608, "x":3.61545, "y":5.97525, "heading":1.57081, "vx":-0.49711, "vy":0.0, "omega":0.001, "ax":-7.51902, "ay":0.0, "alpha":0.01878, "fx":[-124.31553,-124.31487,-124.31487,-124.31553], "fy":[-0.17659,-0.17708,0.17709,0.17658]},
+    {"t":0.09912, "x":3.59493, "y":5.97525, "heading":1.57084, "vx":-0.74553, "vy":0.0, "omega":0.00162, "ax":-7.51428, "ay":0.0, "alpha":0.0234, "fx":[-124.23738,-124.23628,-124.23628,-124.23738], "fy":[-0.21988,-0.22064,0.22066,0.21986]},
+    {"t":0.13216, "x":3.56619, "y":5.97525, "heading":1.5709, "vx":-0.9938, "vy":0.0, "omega":0.00239, "ax":-7.50471, "ay":0.0, "alpha":0.03275, "fx":[-124.07981,-124.07749,-124.07749,-124.07981], "fy":[-0.30707,-0.30853,0.30859,0.30701]},
+    {"t":0.1652, "x":3.52926, "y":5.97525, "heading":1.57098, "vx":-1.24176, "vy":0.0, "omega":0.00347, "ax":-7.47531, "ay":0.0, "alpha":0.0615, "fx":[-123.59709,-123.58821,-123.58821,-123.59709], "fy":[-0.57325,-0.57837,0.57857,0.57304]},
+    {"t":0.19824, "x":3.48415, "y":5.97525, "heading":1.57109, "vx":-1.48874, "vy":0.0, "omega":0.00551, "ax":-0.32678, "ay":0.0, "alpha":6.42191, "fx":[-35.62603,24.83843,24.81977,-35.64318], "fy":[-29.72805,-30.99061,31.00853,29.71013]},
+    {"t":0.23128, "x":3.43479, "y":5.97525, "heading":1.57127, "vx":-1.49954, "vy":0.0, "omega":0.21769, "ax":-0.00001, "ay":0.0, "alpha":5.98527, "fx":[-28.22244,28.24903,28.22206,-28.24941], "fy":[-28.2492,-28.22227,28.24924,28.22223]},
+    {"t":0.26432, "x":3.38524, "y":5.97525, "heading":1.57847, "vx":-1.49954, "vy":0.0, "omega":0.41544, "ax":0.0, "ay":0.0, "alpha":5.53576, "fx":[-25.91408,26.31468,25.91408,-26.31468], "fy":[-26.31468,-25.91408,26.31468,25.91408]},
+    {"t":0.29736, "x":3.3357, "y":5.97525, "heading":1.59219, "vx":-1.49954, "vy":0.0, "omega":0.59834, "ax":0.0, "ay":0.0, "alpha":5.09708, "fx":[-23.52572,24.5546,23.52572,-24.5546], "fy":[-24.5546,-23.52572,24.5546,23.52572]},
+    {"t":0.3304, "x":3.28615, "y":5.97525, "heading":1.61196, "vx":-1.49954, "vy":0.0, "omega":0.76675, "ax":0.0, "ay":0.0, "alpha":4.67107, "fx":[-21.11042,22.92413,21.11042,-22.92413], "fy":[-22.92413,-21.11042,22.92413,21.11042]},
+    {"t":0.36344, "x":3.23661, "y":5.97525, "heading":1.63729, "vx":-1.49954, "vy":0.0, "omega":0.92108, "ax":0.0, "ay":0.0, "alpha":4.25924, "fx":[-18.71351,21.38386,18.71352,-21.38386], "fy":[-21.38387,-18.71352,21.38386,18.71351]},
+    {"t":0.39648, "x":3.18706, "y":5.97525, "heading":1.66773, "vx":-1.49954, "vy":0.0, "omega":1.0618, "ax":0.0, "ay":0.0, "alpha":3.86272, "fx":[-16.37342,19.90054,16.37342,-19.90054], "fy":[-19.90055,-16.37343,19.90052,16.37341]},
+    {"t":0.42952, "x":3.13752, "y":5.97525, "heading":1.70281, "vx":-1.49954, "vy":0.0, "omega":1.18943, "ax":0.0, "ay":0.0, "alpha":3.48226, "fx":[-14.12238,18.44713,14.12239,-18.44713], "fy":[-18.44715,-14.12241,18.44711,14.12237]},
+    {"t":0.46256, "x":3.08797, "y":5.97525, "heading":1.74211, "vx":-1.49954, "vy":0.0, "omega":1.30448, "ax":0.0, "ay":0.0, "alpha":3.11821, "fx":[-11.98719,17.00264,11.98719,-17.00264], "fy":[-17.00267,-11.98722,17.00261,11.98716]},
+    {"t":0.4956, "x":3.03843, "y":5.97525, "heading":1.78521, "vx":-1.49954, "vy":0.0, "omega":1.40751, "ax":0.0, "ay":0.0, "alpha":2.7705, "fx":[-9.98972,15.55156,9.98973,-15.55156], "fy":[-15.5516,-9.98976,15.55153,9.98969]},
+    {"t":0.52864, "x":2.98888, "y":5.97525, "heading":1.83171, "vx":-1.49954, "vy":0.0, "omega":1.49905, "ax":0.0, "ay":0.0, "alpha":2.43873, "fx":[-8.14757,14.08325,8.14758,-14.08325], "fy":[-14.08329,-8.14762,14.08321,8.14753]},
+    {"t":0.56168, "x":2.93934, "y":5.97525, "heading":1.88124, "vx":-1.49954, "vy":0.0, "omega":1.57962, "ax":0.0, "ay":0.0, "alpha":2.12217, "fx":[-6.47456,12.59117,6.47456,-12.59117], "fy":[-12.59121,-6.4746,12.59113,6.47452]},
+    {"t":0.59472, "x":2.88979, "y":5.97525, "heading":1.93343, "vx":-1.49954, "vy":0.0, "omega":1.64974, "ax":0.0, "ay":0.0, "alpha":1.8198, "fx":[-4.98124,11.07209,4.98124,-11.07209], "fy":[-11.07212,-4.98127,11.07206,4.9812]},
+    {"t":0.62776, "x":2.84025, "y":5.97525, "heading":1.98794, "vx":-1.49954, "vy":0.0, "omega":1.70986, "ax":0.0, "ay":0.0, "alpha":1.53035, "fx":[-3.67545,9.52539,3.67545,-9.52538], "fy":[-9.5254,-3.67547,9.52537,3.67543]},
+    {"t":0.6608, "x":2.7907, "y":5.97525, "heading":2.04443, "vx":-1.49954, "vy":0.0, "omega":1.76043, "ax":0.0, "ay":0.0, "alpha":1.25233, "fx":[-2.56281,7.95232,2.56281,-7.95232], "fy":[-7.95231,-2.56281,7.95232,2.56281]},
+    {"t":0.69384, "x":2.74116, "y":5.97525, "heading":2.1026, "vx":-1.49954, "vy":0.0, "omega":1.8018, "ax":0.0, "ay":0.0, "alpha":0.98409, "fx":[-1.6472,6.35547,1.6472,-6.35547], "fy":[-6.35544,-1.64717,6.35549,1.64722]},
+    {"t":0.72688, "x":2.69161, "y":5.97525, "heading":2.16213, "vx":-1.49954, "vy":0.0, "omega":1.83432, "ax":0.0, "ay":0.0, "alpha":0.7238, "fx":[-0.93126,4.73825,0.93126,-4.73825], "fy":[-4.7382,-0.93121,4.7383,0.93131]},
+    {"t":0.75992, "x":2.64207, "y":5.97525, "heading":2.22273, "vx":-1.49954, "vy":0.0, "omega":1.85823, "ax":0.0, "ay":0.0, "alpha":0.46952, "fx":[-0.41682,3.10459,0.41682,-3.10459], "fy":[-3.10452,-0.41675,3.10466,0.41689]},
+    {"t":0.79296, "x":2.59252, "y":5.97525, "heading":2.28413, "vx":-1.49954, "vy":0.0, "omega":1.87374, "ax":0.0, "ay":0.0, "alpha":0.21921, "fx":[-0.1053,1.45871,0.1053,-1.45871], "fy":[-1.45863,-0.10522,1.4588,0.10539]},
+    {"t":0.826, "x":2.54298, "y":5.97525, "heading":2.34604, "vx":-1.49954, "vy":0.0, "omega":1.88099, "ax":0.0, "ay":0.00001, "alpha":-0.02922, "fx":[0.00198,-0.19491,-0.00198,0.19491], "fy":[0.19499,0.00206,-0.19482,-0.0019]},
+    {"t":0.85904, "x":2.49344, "y":5.97525, "heading":2.40819, "vx":-1.49954, "vy":0.0, "omega":1.88002, "ax":0.0, "ay":0.0, "alpha":-0.2779, "fx":[-0.09635,-1.85155,0.09635,1.85155], "fy":[1.85163,-0.09628,-1.85148,0.09643]},
+    {"t":0.89208, "x":2.44389, "y":5.97525, "heading":2.4703, "vx":-1.49954, "vy":0.0, "omega":1.87084, "ax":0.0, "ay":0.0, "alpha":-0.52897, "fx":[-0.40182,-3.50614,0.40182,3.50614], "fy":[3.5062,-0.40176,-3.50608,0.40188]},
+    {"t":0.92512, "x":2.39435, "y":5.97525, "heading":2.53211, "vx":-1.49954, "vy":0.0, "omega":1.85336, "ax":0.0, "ay":0.0, "alpha":-0.78451, "fx":[-0.91602,-5.15318,0.91601,5.15317], "fy":[5.15321,-0.91598,-5.15314,0.91605]},
+    {"t":0.95816, "x":2.3448, "y":5.97525, "heading":2.59335, "vx":-1.49954, "vy":0.0, "omega":1.82744, "ax":0.0, "ay":0.0, "alpha":-1.04654, "fx":[-1.64036,-6.78667,1.64036,6.78667], "fy":[6.78668,-1.64035,-6.78666,1.64037]},
+    {"t":0.9912, "x":2.29526, "y":5.97525, "heading":2.65373, "vx":-1.49954, "vy":0.0, "omega":1.79287, "ax":0.0, "ay":0.0, "alpha":-1.31697, "fx":[-2.57581,-8.40023,2.57581,8.40023], "fy":[8.40022,-2.57582,-8.40024,2.5758]},
+    {"t":1.02424, "x":2.24571, "y":5.97525, "heading":2.71296, "vx":-1.49954, "vy":0.0, "omega":1.74935, "ax":0.0, "ay":0.0, "alpha":-1.59758, "fx":[-3.72243,-9.98723,3.72243,9.98723], "fy":[9.9872,-3.72246,-9.98726,3.7224]},
+    {"t":1.05728, "x":2.19617, "y":5.97525, "heading":2.77076, "vx":-1.49954, "vy":0.0, "omega":1.69657, "ax":0.0, "ay":0.0, "alpha":-1.88999, "fx":[-5.07894,-11.54112,5.07894,11.54112], "fy":[11.54108,-5.07898,-11.54116,5.0789]},
+    {"t":1.09032, "x":2.14662, "y":5.97525, "heading":2.82682, "vx":-1.49954, "vy":0.0, "omega":1.63412, "ax":0.0, "ay":0.0, "alpha":-2.19564, "fx":[-6.64219,-13.05592,6.64219,13.05591], "fy":[13.05587,-6.64224,-13.05596,6.64215]},
+    {"t":1.12336, "x":2.09708, "y":5.97525, "heading":2.88081, "vx":-1.49954, "vy":0.0, "omega":1.56158, "ax":0.0, "ay":0.0, "alpha":-2.51573, "fx":[-8.40672,-14.52678,8.40672,14.52678], "fy":[14.52674,-8.40676,-14.52682,8.40668]},
+    {"t":1.1564, "x":2.04753, "y":5.97525, "heading":2.9324, "vx":-1.49954, "vy":0.0, "omega":1.47846, "ax":0.0, "ay":0.0, "alpha":-2.85122, "fx":[-10.36423,-15.95077,10.36422,15.95077], "fy":[15.95073,-10.36426,-15.9508,10.36419]},
+    {"t":1.18944, "x":1.99799, "y":5.97525, "heading":2.98125, "vx":-1.49954, "vy":0.0, "omega":1.38426, "ax":0.0, "ay":0.0, "alpha":-3.20277, "fx":[-12.50312,-17.32763,12.50312,17.32763], "fy":[17.32761,-12.50315,-17.32766,12.5031]},
+    {"t":1.22248, "x":1.94844, "y":5.97525, "heading":3.02699, "vx":-1.49954, "vy":0.0, "omega":1.27844, "ax":0.0, "ay":0.0, "alpha":-3.57071, "fx":[-14.80815,-18.66074,14.80814,18.66074], "fy":[18.66072,-14.80816,-18.66075,14.80813]},
+    {"t":1.25552, "x":1.8989, "y":5.97525, "heading":3.06923, "vx":-1.49954, "vy":0.0, "omega":1.16046, "ax":0.0, "ay":0.0, "alpha":-3.955, "fx":[-17.25998,-19.958,17.26,19.95802], "fy":[19.958,-17.26,-19.95801,17.25998]},
+    {"t":1.28856, "x":1.84935, "y":5.97525, "heading":3.10757, "vx":-1.49954, "vy":0.0, "omega":1.02979, "ax":0.02138, "ay":0.0, "alpha":-4.35513, "fx":[-19.47966,-20.88008,20.18937,21.58456], "fy":[21.25187,-19.85405,-21.21348,19.81566]},
+    {"t":1.3216, "x":1.79982, "y":5.97525, "heading":3.14159, "vx":-1.49883, "vy":0.0, "omega":0.88589, "ax":-0.01864, "ay":0.0, "alpha":-5.03038, "fx":[-24.03907,-24.03907,23.42261,23.42261], "fy":[23.70877,-23.70877,-23.75353,23.75353]},
+    {"t":1.36604, "x":1.73319, "y":5.97525, "heading":-3.10222, "vx":-1.49966, "vy":0.0, "omega":0.66233, "ax":0.0, "ay":0.0, "alpha":-4.45175, "fx":[-21.81163,-20.15838,21.81162,20.15837], "fy":[20.15837,-21.81163,-20.15837,21.81163]},
+    {"t":1.41048, "x":1.66654, "y":5.97525, "heading":-3.07279, "vx":-1.49966, "vy":0.0, "omega":0.46449, "ax":0.0, "ay":0.0, "alpha":-3.89662, "fx":[-19.6028,-17.07513,19.6028,17.07513], "fy":[17.07513,-19.6028,-17.07513,19.6028]},
+    {"t":1.45492, "x":1.59989, "y":5.97525, "heading":-3.05214, "vx":-1.49966, "vy":0.0, "omega":0.29132, "ax":0.0, "ay":0.0, "alpha":-3.36526, "fx":[-17.23041,-14.39406,17.23041,14.39406], "fy":[14.39406,-17.23041,-14.39406,17.23041]},
+    {"t":1.49937, "x":1.53325, "y":5.97525, "heading":-3.0392, "vx":-1.49966, "vy":0.0, "omega":0.14176, "ax":0.0, "ay":0.0, "alpha":-2.85697, "fx":[-14.78491,-12.02958,14.78491,12.02958], "fy":[12.02958,-14.78491,-12.02958,14.78491]},
+    {"t":1.54381, "x":1.4666, "y":5.97525, "heading":-3.0329, "vx":-1.49966, "vy":0.0, "omega":0.01479, "ax":0.0, "ay":0.0, "alpha":-2.37028, "fx":[-12.3289,-9.90284,12.3289,9.90284], "fy":[9.90284,-12.3289,-9.90284,12.3289]},
+    {"t":1.58825, "x":1.39995, "y":5.97525, "heading":-3.03224, "vx":-1.49966, "vy":0.0, "omega":-0.09055, "ax":0.0, "ay":0.0, "alpha":-1.90297, "fx":[-9.90346,-7.94397,9.90346,7.94397], "fy":[7.94397,-9.90346,-7.94397,9.90346]},
+    {"t":1.63269, "x":1.3333, "y":5.97525, "heading":-3.03626, "vx":-1.49966, "vy":0.0, "omega":-0.17512, "ax":0.0, "ay":0.0, "alpha":-1.45224, "fx":[-7.53328,-6.09274,7.53328,6.09274], "fy":[6.09274,-7.53328,-6.09274,7.53328]},
+    {"t":1.67713, "x":1.26665, "y":5.97525, "heading":-3.04405, "vx":-1.49966, "vy":0.0, "omega":-0.23966, "ax":0.0, "ay":0.0, "alpha":-1.01476, "fx":[-5.23065,-4.29819,5.23065,4.29819], "fy":[4.29819,-5.23065,-4.29819,5.23065]},
+    {"t":1.72158, "x":1.20001, "y":5.97525, "heading":-3.0547, "vx":-1.49966, "vy":0.0, "omega":-0.28476, "ax":0.0, "ay":0.0, "alpha":-0.58688, "fx":[-2.99846,-2.5179,2.99846,2.5179], "fy":[2.5179,-2.99846,-2.5179,2.99846]},
+    {"t":1.76602, "x":1.13336, "y":5.97525, "heading":-3.06735, "vx":-1.49966, "vy":0.0, "omega":-0.31085, "ax":0.0, "ay":0.0, "alpha":-0.16467, "fx":[-0.8323,-0.71706,0.8323,0.71706], "fy":[0.71706,-0.8323,-0.71706,0.8323]},
+    {"t":1.81046, "x":1.06671, "y":5.97525, "heading":-3.08117, "vx":-1.49966, "vy":0.0, "omega":-0.31816, "ax":0.0, "ay":0.0, "alpha":0.25594, "fx":[1.27812,1.13229,-1.27812,-1.13229], "fy":[-1.13229,1.27812,1.13229,-1.27812]},
+    {"t":1.8549, "x":1.00006, "y":5.97525, "heading":-3.09531, "vx":-1.49966, "vy":0.0, "omega":-0.30679, "ax":0.0, "ay":0.0, "alpha":0.67904, "fx":[3.34819,3.05176,-3.34819,-3.05176], "fy":[-3.05176,3.34819,3.05176,-3.34819]},
+    {"t":1.89934, "x":0.93341, "y":5.97525, "heading":-3.10894, "vx":-1.49966, "vy":0.0, "omega":-0.27661, "ax":0.0, "ay":0.0, "alpha":1.10868, "fx":[5.39818,5.05671,-5.39819,-5.05671], "fy":[-5.05671,5.39819,5.05671,-5.39819]},
+    {"t":1.94379, "x":0.86677, "y":5.97525, "heading":-3.12124, "vx":-1.49966, "vy":0.0, "omega":-0.22734, "ax":0.0, "ay":0.0, "alpha":1.54873, "fx":[7.45339,7.15594,-7.45339,-7.15594], "fy":[-7.15594,7.45339,7.15594,-7.45339]},
+    {"t":1.98823, "x":0.80012, "y":5.97525, "heading":-3.13134, "vx":-1.49966, "vy":0.0, "omega":-0.15851, "ax":0.00004, "ay":0.0, "alpha":2.00279, "fx":[9.54528,9.35152,-9.54395,-9.35019], "fy":[-9.35085,9.54461,9.35086,-9.54462]},
+    {"t":2.03267, "x":0.73347, "y":5.97525, "heading":-3.13838, "vx":-1.49966, "vy":0.0, "omega":-0.0695, "ax":3.67499, "ay":0.0, "alpha":1.53457, "fx":[66.28802,66.25454,55.23046,55.26796], "fy":[-8.43326,8.49016,9.44517,-9.50206]},
+    {"t":2.07711, "x":0.67045, "y":5.97525, "heading":-3.14147, "vx":-1.33634, "vy":0.0, "omega":-0.0013, "ax":7.50288, "ay":0.0, "alpha":0.01216, "fx":[124.04882,124.04882,124.0479,124.0479], "fy":[-0.11416,0.11418,0.11437,-0.11439]},
+    {"t":2.12156, "x":0.61847, "y":5.97525, "heading":-3.14153, "vx":-1.00289, "vy":0.0, "omega":-0.00076, "ax":7.51792, "ay":0.0, "alpha":0.00712, "fx":[124.29723,124.29723,124.29696,124.29696], "fy":[-0.06701,0.06702,0.06708,-0.06709]},
+    {"t":2.166, "x":0.58132, "y":5.97525, "heading":-3.14156, "vx":-0.66878, "vy":0.0, "omega":-0.00045, "ax":7.52296, "ay":0.0, "alpha":0.00544, "fx":[124.38041,124.38041,124.38027,124.38027], "fy":[-0.0512,0.0512,0.05124,-0.05124]},
+    {"t":2.21044, "x":0.55903, "y":5.97525, "heading":-3.14158, "vx":-0.33445, "vy":0.0, "omega":-0.0002, "ax":7.52548, "ay":0.0, "alpha":0.00459, "fx":[124.42206,124.42206,124.42198,124.42198], "fy":[-0.04328,0.04328,0.04331,-0.04331]},
+    {"t":2.25488, "x":0.5516, "y":5.97525, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 9275ac07..bb97a6db 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -319,7 +319,7 @@ public void lockHoodUnderTrench(AutoRoutine routine, Pose2d trench, double toler
   }
 
   public Command shootPreload() {
-    return Commands.sequence(setAutoScoreReqTrue(), waitUntilEmpty(), setAutoScoreReqFalse());
+    return Commands.sequence(setAutoScoreReqTrue(), swerve.stop().repeatedly().withTimeout(3));
   }
 
   public Command setAutoIntakeReqTrue() {
@@ -557,7 +557,11 @@ public Command getBumpOutpostClimbAuto() {
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
     Path[] paths = {Path.BtoD, Path.DtoS, Path.StoO, Path.OtoCR};
     Command autoCommand =
-        paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse());
+        paths[0]
+            .getTrajectory(routine)
+            .resetOdometry()
+            .alongWith(setleftClimbAutoFalse())
+            .andThen(shootPreload());
 
     for (Path p : paths) {
       autoCommand = autoCommand.andThen(runPath(p, routine));

From 798c2a7bc684bf542670453b485e4cd4bee478f6 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 18:21:19 -0700
Subject: [PATCH 45/54] Update at 'Sat Mar 14 18:21:18 PDT 2026'

---
 .../java/frc/robot/utils/autoaim/AutoAim.java | 42 +++++++++----------
 1 file changed, 21 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index e25a777a..11ea1f2d 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -128,49 +128,49 @@ public class AutoAim {
 
   static { // For feed shot tree
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20, 0)); // - 2, 0));
+        Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0)); // - 2, 0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); // - 2, 0.0));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38, 1.42)); // - 2, 1.42));
+        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42)); // - 2, 1.42));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40, 1.36)); // - 2, 1.36));
+        Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36)); // - 2, 1.36));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41, 1.34)); // - 2, 1.34));
+        Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34)); // - 2, 1.34));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42, 1.25)); // - 2, 1.25));
+        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25)); // - 2, 1.25));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43, 1.28)); // - 2, 1.28));
+        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28)); // - 2, 1.28));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45, 1.27)); // - 2, 1.27));
+        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27)); // - 2, 1.27));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46, 1.32)); // - 2, 1.32));
+        Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32)); // - 2, 1.32));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48, 1.4)); // - 2, 1.4));
+        Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4)); // - 2, 1.4));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49, 1.3)); // - 2, 1.3));
+        Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53, 1.33)); // - 2, 1.33));
+        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33)); // - 2, 1.33));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57, 1.3)); // - 2, 1.3));
+        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57, 1.2)); // - 2, 1.2));
+        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2)); // - 2, 1.2));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59, 1.2)); // - 2, 1.2));
+        Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2)); // - 2, 1.2));
 
     // TODO: POPULATE beyond 24 feet and time of flight
   }

From f85159dd46ee50c8db0bb3ad3c84ae42fee40159 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 22:34:44 -0700
Subject: [PATCH 46/54] right bump one

---
 src/main/deploy/choreo/RBtoO.traj             | 100 ++++++++++++++++++
 src/main/java/frc/robot/Autos.java            |  75 +++++++++++--
 src/main/java/frc/robot/Robot.java            |   4 +-
 .../java/frc/robot/utils/autoaim/AutoAim.java |  60 +++++++----
 4 files changed, 207 insertions(+), 32 deletions(-)
 create mode 100644 src/main/deploy/choreo/RBtoO.traj

diff --git a/src/main/deploy/choreo/RBtoO.traj b/src/main/deploy/choreo/RBtoO.traj
new file mode 100644
index 00000000..1a8ab342
--- /dev/null
+++ b/src/main/deploy/choreo/RBtoO.traj
@@ -0,0 +1,100 @@
+{
+ "name":"RBtoO",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.5344297885894775, "y":2.057757616043091, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"3.5344297885894775 m", "val":3.5344297885894775}, "y":{"exp":"2.057757616043091 m", "val":2.057757616043091}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2761996,
+    "y":0.2761996
+   },
+   "backLeft":{
+    "x":-0.2761996,
+    "y":0.2761996
+   },
+   "mass":66.133767546,
+   "inertia":10.42384825260472,
+   "gearing":7.03,
+   "radius":0.0508,
+   "vmax":485.8996637552213,
+   "tmax":0.9,
+   "cof":2.25,
+   "bumper":{
+    "front":0.44196,
+    "side":0.44196,
+    "back":0.44196
+   },
+   "differentialTrackWidth":0.5427218
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.99223],
+  "samples":[
+    {"t":0.0, "x":3.53443, "y":2.05776, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.76463, "ay":-3.29982, "alpha":0.0, "fx":[-111.84269,-111.84269,-111.84269,-111.84269], "fy":[-54.55744,-54.55744,-54.55744,-54.55744]},
+    {"t":0.04981, "x":3.52604, "y":2.05366, "heading":1.5708, "vx":-0.33692, "vy":-0.16435, "omega":0.0, "ax":-6.76318, "ay":-3.29911, "alpha":0.0, "fx":[-111.81859,-111.81859,-111.81859,-111.81859], "fy":[-54.54568,-54.54568,-54.54568,-54.54568]},
+    {"t":0.09961, "x":3.50087, "y":2.04139, "heading":1.5708, "vx":-0.67376, "vy":-0.32867, "omega":0.0, "ax":-6.76075, "ay":-3.29793, "alpha":0.0, "fx":[-111.77842,-111.77842,-111.77842,-111.77842], "fy":[-54.52609,-54.52609,-54.52609,-54.52609]},
+    {"t":0.14942, "x":3.45893, "y":2.02093, "heading":1.5708, "vx":-1.01049, "vy":-0.49292, "omega":0.0, "ax":-6.75589, "ay":-3.29556, "alpha":0.0, "fx":[-111.69815,-111.69815,-111.69815,-111.69815], "fy":[-54.48693,-54.48693,-54.48693,-54.48693]},
+    {"t":0.19922, "x":3.40022, "y":1.99229, "heading":1.5708, "vx":-1.34697, "vy":-0.65706, "omega":0.0, "ax":-6.74136, "ay":-3.28847, "alpha":0.0, "fx":[-111.45787,-111.45787,-111.45787,-111.45787], "fy":[-54.36972,-54.36972,-54.36972,-54.36972]},
+    {"t":0.24903, "x":3.32477, "y":1.95549, "heading":1.5708, "vx":-1.68273, "vy":-0.82085, "omega":0.0, "ax":-2.29774, "ay":-1.12085, "alpha":0.0, "fx":[-37.98949,-37.98949,-37.98949,-37.98949], "fy":[-18.53147,-18.53147,-18.53147,-18.53147]},
+    {"t":0.29884, "x":3.23811, "y":1.91321, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":-0.00002, "ay":-0.00001, "alpha":0.0, "fx":[-0.0004,-0.0004,-0.0004,-0.0004], "fy":[-0.0002,-0.0002,-0.0002,-0.0002]},
+    {"t":0.34864, "x":3.1486, "y":1.86955, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39845, "x":3.05909, "y":1.82589, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.44825, "x":2.96958, "y":1.78222, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.49806, "x":2.88007, "y":1.73856, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.54786, "x":2.79056, "y":1.6949, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.59767, "x":2.70105, "y":1.65123, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.64748, "x":2.61154, "y":1.60757, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.69728, "x":2.52203, "y":1.56391, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.74709, "x":2.43252, "y":1.52024, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.79689, "x":2.34301, "y":1.47658, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.8467, "x":2.2535, "y":1.43292, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.89651, "x":2.16399, "y":1.38925, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.94631, "x":2.07448, "y":1.34559, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.99612, "x":1.98497, "y":1.30193, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.04592, "x":1.89547, "y":1.25826, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.09573, "x":1.80596, "y":1.2146, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.14553, "x":1.71645, "y":1.17094, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.19534, "x":1.62694, "y":1.12727, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.24515, "x":1.53743, "y":1.08361, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.29495, "x":1.44792, "y":1.03995, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.34476, "x":1.35841, "y":0.99628, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.39456, "x":1.2689, "y":0.95262, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.44437, "x":1.17939, "y":0.90896, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.49418, "x":1.08988, "y":0.86529, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.54398, "x":1.00037, "y":0.82163, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.59379, "x":0.91086, "y":0.77797, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.64359, "x":0.82135, "y":0.7343, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":0.00002, "ay":0.00001, "alpha":0.0, "fx":[0.0004,0.0004,0.0004,0.0004], "fy":[0.0002,0.0002,0.0002,0.0002]},
+    {"t":1.6934, "x":0.73184, "y":0.69064, "heading":1.5708, "vx":-1.79717, "vy":-0.87667, "omega":0.0, "ax":2.29774, "ay":1.12085, "alpha":0.0, "fx":[37.98949,37.98949,37.98949,37.98949], "fy":[18.53147,18.53147,18.53147,18.53147]},
+    {"t":1.74321, "x":0.64518, "y":0.64837, "heading":1.5708, "vx":-1.68273, "vy":-0.82085, "omega":0.0, "ax":6.74136, "ay":3.28847, "alpha":0.0, "fx":[111.45787,111.45787,111.45787,111.45787], "fy":[54.36972,54.36972,54.36972,54.36972]},
+    {"t":1.79301, "x":0.56973, "y":0.61156, "heading":1.5708, "vx":-1.34697, "vy":-0.65706, "omega":0.0, "ax":6.75589, "ay":3.29556, "alpha":0.0, "fx":[111.69815,111.69815,111.69815,111.69815], "fy":[54.48693,54.48693,54.48693,54.48693]},
+    {"t":1.84282, "x":0.51102, "y":0.58292, "heading":1.5708, "vx":-1.01049, "vy":-0.49292, "omega":0.0, "ax":6.76075, "ay":3.29793, "alpha":0.0, "fx":[111.77842,111.77842,111.77842,111.77842], "fy":[54.52609,54.52609,54.52609,54.52609]},
+    {"t":1.89262, "x":0.46908, "y":0.56246, "heading":1.5708, "vx":-0.67376, "vy":-0.32867, "omega":0.0, "ax":6.76318, "ay":3.29911, "alpha":0.0, "fx":[111.81859,111.81859,111.81859,111.81859], "fy":[54.54568,54.54568,54.54568,54.54568]},
+    {"t":1.94243, "x":0.44391, "y":0.55019, "heading":1.5708, "vx":-0.33692, "vy":-0.16435, "omega":0.0, "ax":6.76463, "ay":3.29982, "alpha":0.0, "fx":[111.84269,111.84269,111.84269,111.84269], "fy":[54.55744,54.55744,54.55744,54.55744]},
+    {"t":1.99223, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index bb97a6db..2f287952 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -64,9 +64,11 @@ public enum Action {
     INTAKE,
     SCORE,
     FLOW,
-    CLIMB,
+    CLIMB_SCORE,
     OUTPOST,
-    NOTHING;
+    NOTHING,
+    OUTPOST_SCORE,
+    CLIMB_ONLY;
   }
 
   public enum Obstacle {
@@ -131,10 +133,12 @@ public enum Path {
     // FLOW
     MLtoD("ML", "D", Action.FLOW),
     // CLIMB
-    MLtoCL("ML", "CL", Action.CLIMB),
-    MRtoCR("MR", "CR", Action.CLIMB),
-    OtoCR("O", "CR", Action.CLIMB),
-    DtoCL("D", "CL", Action.CLIMB),
+    MLtoCL("ML", "CL", Action.CLIMB_SCORE),
+    MRtoCR("MR", "CR", Action.CLIMB_SCORE),
+    OtoCR("O", "CR", Action.CLIMB_SCORE),
+    noScoreOtoCR("O", "CR", Action.CLIMB_ONLY),
+    DtoCL("D", "CL", Action.CLIMB_SCORE),
+    RBtoO("RB", "O", Action.OUTPOST_SCORE),
 
     RUNtoTEST("RUN", "TEST", Action.NOTHING),
 
@@ -204,12 +208,16 @@ public Command runPath(Path path, AutoRoutine routine) {
         return feedPath(path, routine);
       case SCORE:
         return scorePath(path, routine);
-      case CLIMB:
-        return climbPath(path, routine);
+      case CLIMB_SCORE:
+        return climbScorePath(path, routine);
       case FLOW:
         return flowPath(path, routine);
       case OUTPOST:
         return outpostPath(path, routine);
+      case OUTPOST_SCORE:
+        return outpostScorePath(path, routine);
+      case CLIMB_ONLY:
+        return climbNoScorePath(path, routine);
       case NOTHING:
         return emptyPath(path, routine);
       default: // this should never happen
@@ -217,7 +225,7 @@ public Command runPath(Path path, AutoRoutine routine) {
     }
   }
 
-  public Command climbPath(Path path, AutoRoutine routine) {
+  public Command climbScorePath(Path path, AutoRoutine routine) {
     return Commands.sequence(
         setAutoScoreReqFalse(),
         setAutoIntakeReqFalse(),
@@ -244,6 +252,20 @@ public Command climbPath(Path path, AutoRoutine routine) {
                     setAutoClimbReqTrue())));
   }
 
+  public Command climbNoScorePath(Path path, AutoRoutine routine) {
+    return Commands.sequence(
+        setAutoScoreReqFalse(),
+        setAutoIntakeReqFalse(),
+        setAutoPreClimbReqTrue(),
+        // Commands.parallel(
+        path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()),
+        swerve.stop().until(() -> climber.atFullExtension()),
+        Commands.parallel(
+            swerve.alignToClimb(() -> getClimbAutoTarget()),
+            Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
+                .andThen(setAutoClimbReqTrue())));
+  }
+
   public Command feedPath(Path path, AutoRoutine routine) {
     return Commands.sequence(
         setAutoScoreReqFalse(),
@@ -309,6 +331,19 @@ public Command outpostPath(Path path, AutoRoutine routine) {
         );
   }
 
+  public Command outpostScorePath(Path path, AutoRoutine routine) {
+    return Commands.sequence(
+        setAutoScoreReqFalse(),
+        setAutoFlowReqFalse(),
+        setAutoIntakeReqFalse(),
+        path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()),
+        setAutoScoreReqTrue(),
+        swerve.stop().repeatedly().withTimeout(4),
+        setAutoScoreReqFalse()
+        // Commands.waitSeconds(1)
+        );
+  }
+
   public void lockHoodUnderTrench(AutoRoutine routine, Pose2d trench, double tolerance) {
     routine
         .observe(
@@ -552,8 +587,8 @@ public Command getDepotOutpostClimbAuto() {
     return routine.cmd();
   }
 
-  public Command getBumpOutpostClimbAuto() {
-    final AutoRoutine routine = factory.newRoutine("Bump Outpost Climb Auto");
+  public Command getLeftBumpDepotOutpostClimbAuto() {
+    final AutoRoutine routine = factory.newRoutine("Left Bump Outpost Climb Auto");
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
     Path[] paths = {Path.BtoD, Path.DtoS, Path.StoO, Path.OtoCR};
     Command autoCommand =
@@ -572,6 +607,24 @@ public Command getBumpOutpostClimbAuto() {
     return routine.cmd();
   }
 
+  // this is so cursed and im not proud of it
+  public Command getRightBumpOutpostClimbAuto() {
+    final AutoRoutine routine = factory.newRoutine("Right Bump Outpost Climb Auto");
+    lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
+    // Path[] paths = {Path.BtoD, Path.DtoS, Path.StoO, Path.OtoCR};
+    Path[] paths = {Path.RBtoO, Path.noScoreOtoCR};
+    Command autoCommand =
+        paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse());
+
+    for (Path p : paths) {
+      autoCommand = autoCommand.andThen(runPath(p, routine));
+    }
+
+    routine.active().whileTrue(autoCommand);
+
+    return routine.cmd();
+  }
+
   public Command getCenterScoreAuto() {
     final AutoRoutine routine = factory.newRoutine("Center Score Auto");
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 2123902b..88eac0d5 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -757,7 +757,9 @@ private void addAutos() {
     autoChooser.addOption("Score in Center", autos.getCenterScoreAuto());
     autoChooser.addOption("Test Auto", autos.getTestAuto());
     autoChooser.addOption("Just Score", autos.getJustScoreAuto());
-    autoChooser.addOption("Bump Depot Outpost Climb", autos.getBumpOutpostClimbAuto());
+    autoChooser.addOption(
+        "Left Bump Depot Outpost Climb", autos.getLeftBumpDepotOutpostClimbAuto());
+    autoChooser.addOption("Right Bump Outpost Climb", autos.getRightBumpOutpostClimbAuto());
 
     haveAutosGenerated = true;
     System.out.println("Done generating autos");
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 11ea1f2d..5a70e116 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -130,47 +130,67 @@ public class AutoAim {
     FEED_SHOT_TREE.put(
         Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0)); // - 2, 0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(4),
+        new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(6),
+        new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(8),
+        new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(10),
+        new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(12),
+        new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(14),
+        new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); // - 2, 0.0));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); // - 2, 0.0));
+        Units.feetToMeters(16),
+        new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); // - 2, 0.0));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42)); // - 2, 1.42));
+        Units.feetToMeters(18),
+        new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42)); // - 2, 1.42));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36)); // - 2, 1.36));
+        Units.feetToMeters(20),
+        new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36)); // - 2, 1.36));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34)); // - 2, 1.34));
+        Units.feetToMeters(22),
+        new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34)); // - 2, 1.34));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25)); // - 2, 1.25));
+        Units.feetToMeters(24),
+        new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25)); // - 2, 1.25));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28)); // - 2, 1.28));
+        Units.feetToMeters(26),
+        new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28)); // - 2, 1.28));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27)); // - 2, 1.27));
+        Units.feetToMeters(28),
+        new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27)); // - 2, 1.27));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32)); // - 2, 1.32));
+        Units.feetToMeters(30),
+        new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32)); // - 2, 1.32));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4)); // - 2, 1.4));
+        Units.feetToMeters(32),
+        new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4)); // - 2, 1.4));
 
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3)); // - 2, 1.3));
+        Units.feetToMeters(34),
+        new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33)); // - 2, 1.33));
+        Units.feetToMeters(36),
+        new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33)); // - 2, 1.33));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3)); // - 2, 1.3));
+        Units.feetToMeters(38),
+        new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3)); // - 2, 1.3));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2)); // - 2, 1.2));
+        Units.feetToMeters(40),
+        new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2)); // - 2, 1.2));
     FEED_SHOT_TREE.put(
-        Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2)); // - 2, 1.2));
+        Units.feetToMeters(42),
+        new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2)); // - 2, 1.2));
 
     // TODO: POPULATE beyond 24 feet and time of flight
   }

From be1a917be47b1ae231705531e27ab7059d1cb5b9 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 23:16:39 -0700
Subject: [PATCH 47/54] right bump outpost center

---
 src/main/deploy/choreo/RRtoFR.traj | 199 +++++++++++++++--------------
 src/main/java/frc/robot/Autos.java |  46 ++++++-
 src/main/java/frc/robot/Robot.java |   1 +
 3 files changed, 150 insertions(+), 96 deletions(-)

diff --git a/src/main/deploy/choreo/RRtoFR.traj b/src/main/deploy/choreo/RRtoFR.traj
index 4292f558..32144ae2 100644
--- a/src/main/deploy/choreo/RRtoFR.traj
+++ b/src/main/deploy/choreo/RRtoFR.traj
@@ -3,29 +3,29 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":2.5106818675994873, "y":0.6606601476669312, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":6.375746250152588, "y":0.6403176784515381, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":7.532798290252685, "y":1.1749255657196045, "heading":1.0405808793467952, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":2.5106818675994873, "y":0.6606601476669312, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.375746250152588, "y":0.6403176784515381, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":7.532798290252685, "y":1.1749255657196045, "heading":1.0405808793467952, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
     {"x":7.82036542892456, "y":2.2169458866119385, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
     {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}],
   "targetDt":0.05
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"RR.x", "val":2.5106818675994873}, "y":{"exp":"RR.y", "val":0.6606601476669312}, "heading":{"exp":"RR.heading", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"6.375746250152588 m", "val":6.375746250152588}, "y":{"exp":"0.6403176784515381 m", "val":0.6403176784515381}, "heading":{"exp":"RR.heading", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"7.5327982902526855 m", "val":7.532798290252685}, "y":{"exp":"1.1749255657196045 m", "val":1.1749255657196045}, "heading":{"exp":"1.0405808793467952 rad", "val":1.0405808793467952}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"RR.x", "val":2.5106818675994873}, "y":{"exp":"RR.y", "val":0.6606601476669312}, "heading":{"exp":"RR.heading", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.375746250152588 m", "val":6.375746250152588}, "y":{"exp":"0.6403176784515381 m", "val":0.6403176784515381}, "heading":{"exp":"RR.heading", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"7.5327982902526855 m", "val":7.532798290252685}, "y":{"exp":"1.1749255657196045 m", "val":1.1749255657196045}, "heading":{"exp":"1.0405808793467952 rad", "val":1.0405808793467952}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
     {"x":{"exp":"FR.x", "val":7.82036542892456}, "y":{"exp":"FR.y", "val":2.2169458866119385}, "heading":{"exp":"FR.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.0}}}, "enabled":true},
     {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}],
   "targetDt":{
    "exp":"0.05 s",
@@ -57,92 +57,105 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.28888,1.7385,2.11235],
+  "waypoints":[0.0,1.93341,2.57629,3.12125],
   "samples":[
-    {"t":0.0, "x":2.51068, "y":0.66066, "heading":0.0, "vx":2.99678, "vy":-0.0123, "omega":-0.104, "ax":0.06333, "ay":-0.00912, "alpha":-0.01465, "fx":[1.11615,1.11615,0.97795,0.97795], "fy":[-0.21994,-0.08172,-0.08172,-0.21995]},
-    {"t":0.03791, "x":2.62433, "y":0.66019, "heading":-0.00394, "vx":2.99918, "vy":-0.01265, "omega":-0.10455, "ax":-0.00007, "ay":-0.01716, "alpha":-0.03127, "fx":[0.14573,0.1469,-0.14809,-0.14925], "fy":[-0.4318,-0.13681,-0.13565,-0.43063]},
-    {"t":0.07582, "x":2.73802, "y":0.6597, "heading":-0.00791, "vx":2.99917, "vy":-0.0133, "omega":-0.10574, "ax":-0.00007, "ay":-0.01494, "alpha":-0.04793, "fx":[0.22318,0.22676,-0.22546,-0.22903], "fy":[-0.47494,-0.02273,-0.01915,-0.47137]},
-    {"t":0.11372, "x":2.85172, "y":0.65918, "heading":-0.01191, "vx":2.99917, "vy":-0.01387, "omega":-0.10756, "ax":-0.00006, "ay":-0.01182, "alpha":-0.06472, "fx":[0.30074,0.30802,-0.30261,-0.30988], "fy":[-0.50436,0.10625,0.11353,-0.49709]},
-    {"t":0.15163, "x":2.96541, "y":0.65865, "heading":-0.01599, "vx":2.99917, "vy":-0.01431, "omega":-0.11001, "ax":-0.00005, "ay":-0.00921, "alpha":-0.08169, "fx":[0.37841,0.39074,-0.37991,-0.39223], "fy":[-0.5437,0.22694,0.23927,-0.53137]},
-    {"t":0.18954, "x":3.0791, "y":0.6581, "heading":-0.02016, "vx":2.99917, "vy":-0.01466, "omega":-0.11311, "ax":-0.00004, "ay":-0.00719, "alpha":-0.09885, "fx":[0.45621,0.47502,-0.45742,-0.47621], "fy":[-0.59454,0.33789,0.35669,-0.57574]},
-    {"t":0.22745, "x":3.19279, "y":0.65754, "heading":-0.02445, "vx":2.99917, "vy":-0.01494, "omega":-0.11685, "ax":-0.00003, "ay":-0.00568, "alpha":-0.11617, "fx":[0.534,0.56081,-0.53498,-0.56177], "fy":[-0.65526,0.44051,0.46731,-0.62847]},
-    {"t":0.26536, "x":3.30649, "y":0.65697, "heading":-0.02888, "vx":2.99917, "vy":-0.01515, "omega":-0.12126, "ax":-0.00002, "ay":-0.00458, "alpha":-0.1336, "fx":[0.61139,0.64779,-0.61219,-0.64858], "fy":[-0.72393,0.53604,0.57244,-0.68753]},
-    {"t":0.30326, "x":3.42018, "y":0.65639, "heading":-0.03348, "vx":2.99917, "vy":-0.01532, "omega":-0.12632, "ax":-0.00002, "ay":-0.00381, "alpha":-0.15098, "fx":[0.68769,0.73537,-0.68836,-0.73603], "fy":[-0.79867,0.62505,0.67273,-0.751]},
-    {"t":0.34117, "x":3.53387, "y":0.6558, "heading":-0.03826, "vx":2.99916, "vy":-0.01547, "omega":-0.13205, "ax":-0.00002, "ay":-0.00331, "alpha":-0.1681, "fx":[0.76182,0.82251,-0.76242,-0.82309], "fy":[-0.87757,0.70735,0.76802,-0.8169]},
-    {"t":0.37908, "x":3.64756, "y":0.65522, "heading":-0.04327, "vx":2.99916, "vy":-0.01559, "omega":-0.13842, "ax":-0.00002, "ay":-0.00306, "alpha":-0.18463, "fx":[0.83222,0.90758,-0.83278,-0.90812], "fy":[-0.95844,0.78191,0.85726,-0.88309]},
-    {"t":0.41699, "x":3.76126, "y":0.65462, "heading":-0.04852, "vx":2.99916, "vy":-0.01571, "omega":-0.14542, "ax":-0.00002, "ay":-0.00303, "alpha":-0.20006, "fx":[0.89664,0.9882,-0.8972,-0.98873], "fy":[-1.0386,0.84679,0.93833,-0.94705]},
-    {"t":0.4549, "x":3.87495, "y":0.65402, "heading":-0.05403, "vx":2.99916, "vy":-0.01583, "omega":-0.153, "ax":-0.00002, "ay":-0.00323, "alpha":-0.21371, "fx":[0.95198,1.06088,-0.95258,-1.06146], "fy":[-1.11457,0.89888,1.00777,-1.00567]},
-    {"t":0.49281, "x":3.98864, "y":0.65342, "heading":-0.05983, "vx":2.99916, "vy":-0.01595, "omega":-0.1611, "ax":-0.00002, "ay":-0.00367, "alpha":-0.22461, "fx":[0.99399,1.12072,-0.99467,-1.12137], "fy":[-1.18167,0.9337,1.06041,-1.05496]},
-    {"t":0.53071, "x":4.10233, "y":0.65282, "heading":-0.06594, "vx":2.99916, "vy":-0.01609, "omega":-0.16962, "ax":-0.00002, "ay":-0.00438, "alpha":-0.2314, "fx":[1.01693,1.1608,-1.01775,-1.16158], "fy":[-1.23356,0.94496,1.08882,-1.08971]},
-    {"t":0.56862, "x":4.21603, "y":0.6522, "heading":-0.07237, "vx":2.99916, "vy":-0.01625, "omega":-0.17839, "ax":-0.00003, "ay":-0.00541, "alpha":-0.23225, "fx":[1.01307,1.17154,-1.01409,-1.1725], "fy":[-1.2615,0.92409,1.08254,-1.10306]},
-    {"t":0.60653, "x":4.32972, "y":0.65158, "heading":-0.07913, "vx":2.99916, "vy":-0.01646, "omega":-0.18719, "ax":-0.00004, "ay":-0.00685, "alpha":-0.22465, "fx":[0.97207,1.13965,-0.97337,-1.14089], "fy":[-1.25346,0.85952,1.02708,-1.08592]},
-    {"t":0.64444, "x":4.44341, "y":0.65095, "heading":-0.08623, "vx":2.99916, "vy":-0.01672, "omega":-0.19571, "ax":-0.00005, "ay":-0.00878, "alpha":-0.20518, "fx":[0.88014,1.04689,-0.88182,-1.0485], "fy":[-1.1929,0.73576,0.90248,-1.0262]},
-    {"t":0.68235, "x":4.5571, "y":0.65031, "heading":-0.09364, "vx":2.99916, "vy":-0.01705, "omega":-0.20349, "ax":-0.00007, "ay":-0.01136, "alpha":-0.16918, "fx":[0.71889,0.86819,-0.7211,-0.87033], "fy":[-1.05707,0.53217,0.68144,-0.90782]},
-    {"t":0.72025, "x":4.6708, "y":0.64966, "heading":-0.10136, "vx":2.99915, "vy":-0.01748, "omega":-0.2099, "ax":-0.00009, "ay":-0.01476, "alpha":-0.11038, "fx":[0.4639,0.56929,-0.46682,-0.57218], "fy":[-0.81476,0.22132,0.3267,-0.70938]},
-    {"t":0.75816, "x":4.78449, "y":0.64899, "heading":-0.10932, "vx":2.99915, "vy":-0.01804, "omega":-0.21409, "ax":-0.00012, "ay":-0.01922, "alpha":-0.02027, "fx":[0.08267,0.10353,-0.08658,-0.10745], "fy":[-0.4233,-0.23319,-0.21233,-0.40244]},
-    {"t":0.79607, "x":4.89818, "y":0.64829, "heading":-0.11743, "vx":2.99915, "vy":-0.01877, "omega":-0.21485, "ax":-0.00016, "ay":-0.02506, "alpha":0.11257, "fx":[-0.46785,-0.59226,0.46249,0.58696], "fy":[0.1752,-0.87957,-1.00399,0.05076]},
-    {"t":0.83398, "x":5.01187, "y":0.64756, "heading":-0.12558, "vx":2.99914, "vy":-0.01972, "omega":-0.21059, "ax":-0.00022, "ay":-0.0327, "alpha":0.30339, "fx":[-1.24457,-1.60282,1.23696,1.59576], "fy":[1.05855,-1.78147,-2.13991,0.69996]},
-    {"t":0.87189, "x":5.12556, "y":0.64679, "heading":-0.13356, "vx":2.99913, "vy":-0.02096, "omega":-0.19909, "ax":-0.00031, "ay":-0.04271, "alpha":0.57232, "fx":[-2.3222,-3.03999,2.31067,3.03102], "fy":[2.32936,-3.02257,-3.74127,1.60999]},
-    {"t":0.90979, "x":5.23926, "y":0.64596, "heading":-0.14111, "vx":2.99912, "vy":-0.02258, "omega":-0.17739, "ax":-0.00044, "ay":-0.05587, "alpha":0.94525, "fx":[-3.79758,-5.04736,3.77847,5.03737], "fy":[4.11875,-4.71194,-5.96488,2.86325]},
-    {"t":0.9477, "x":5.35295, "y":0.64507, "heading":-0.14783, "vx":2.9991, "vy":-0.0247, "omega":-0.14156, "ax":-0.00064, "ay":-0.07335, "alpha":1.4543, "fx":[-5.79352,-7.80061,5.75831,7.79364], "fy":[6.58505,-6.98956,-9.00628,4.56]},
-    {"t":0.98561, "x":5.46664, "y":0.64408, "heading":-0.1532, "vx":2.99908, "vy":-0.02748, "omega":-0.08643, "ax":-0.00095, "ay":-0.09695, "alpha":2.13648, "fx":[-8.46006,-11.49633,8.38857,11.50516], "fy":[9.90023,-10.03042,-13.09377,6.81234]},
-    {"t":1.02352, "x":5.58032, "y":0.64297, "heading":-0.15647, "vx":2.99904, "vy":-0.03115, "omega":-0.00544, "ax":-0.00145, "ay":-0.12942, "alpha":3.02918, "fx":[-11.97037,-16.31749,11.81478,16.37714], "fy":[14.21479,-14.04147,-18.46093,9.72841]},
-    {"t":1.06143, "x":5.69401, "y":0.64169, "heading":-0.15668, "vx":2.99899, "vy":-0.03606, "omega":0.10939, "ax":-0.00228, "ay":-0.17349, "alpha":4.16069, "fx":[-16.50876,-22.3636,16.16106,22.56062], "fy":[19.61267,-19.22552,-25.26513,13.40474]},
-    {"t":1.09933, "x":5.8077, "y":0.6402, "heading":-0.15253, "vx":2.9989, "vy":-0.04263, "omega":0.26712, "ax":-0.00346, "ay":-0.22096, "alpha":5.53815, "fx":[-22.24199,-29.56639,21.51091,30.06888], "fy":[26.20624,-25.57515,-33.32943,18.08561]},
-    {"t":1.13724, "x":5.92138, "y":0.63843, "heading":-0.14241, "vx":2.99877, "vy":-0.05101, "omega":0.47706, "ax":-0.00353, "ay":-0.19248, "alpha":7.14898, "fx":[-29.13142,-37.80286,28.11386,38.58727], "fy":[35.08668,-31.8778,-41.20578,25.26764]},
-    {"t":1.17515, "x":6.03505, "y":0.63635, "heading":-0.12432, "vx":2.99864, "vy":-0.05831, "omega":0.74806, "ax":0.00514, "ay":0.29609, "alpha":8.94568, "fx":[-35.54525,-48.19311,37.90566,46.17284], "fy":[51.67301,-31.40781,-42.39642,41.71268]},
-    {"t":1.21306, "x":6.14873, "y":0.63436, "heading":-0.09596, "vx":2.99883, "vy":-0.04708, "omega":1.08718, "ax":-0.00016, "ay":2.47209, "alpha":9.42093, "fx":[-35.22144,-64.28885,56.44716,43.05228], "fy":[82.25218,7.84392,-3.63247,77.02502]},
-    {"t":1.25097, "x":6.26241, "y":0.63435, "heading":-0.05475, "vx":2.99882, "vy":0.04663, "omega":1.4443, "ax":-0.47374, "ay":5.84847, "alpha":5.20923, "fx":[-33.28541,-59.84626,37.86743,23.93402], "fy":[106.23686,82.45595,90.0433,108.04498]},
-    {"t":1.28888, "x":6.37575, "y":0.64032, "heading":0.0, "vx":2.98087, "vy":0.26833, "omega":1.64178, "ax":-1.89301, "ay":5.77939, "alpha":3.3866, "fx":[-46.59026,-64.81746,-8.38906,-5.39501], "fy":[98.68826,79.36963,96.64301,107.51213]},
-    {"t":1.30686, "x":6.42905, "y":0.64608, "heading":0.02953, "vx":2.94682, "vy":0.37228, "omega":1.70268, "ax":-1.61822, "ay":5.351, "alpha":2.3638, "fx":[-38.60146,-47.62896,-11.28375,-9.50473], "fy":[91.48766,78.15543,86.93758,97.30085]},
-    {"t":1.32485, "x":6.48179, "y":0.65364, "heading":0.06015, "vx":2.91772, "vy":0.46851, "omega":1.7452, "ax":-1.21519, "ay":4.58602, "alpha":1.37675, "fx":[-27.63277,-30.14245,-11.49019,-11.09954], "fy":[78.59158,69.98761,73.32139,81.39048]},
-    {"t":1.34283, "x":6.53406, "y":0.66281, "heading":0.09154, "vx":2.89586, "vy":0.55099, "omega":1.76996, "ax":-0.69332, "ay":3.56592, "alpha":0.52897, "fx":[-14.50702,-14.50054,-8.30305,-8.54096], "fy":[60.49799,56.625,57.43073,61.27404]},
-    {"t":1.36081, "x":6.58603, "y":0.67329, "heading":0.12337, "vx":2.88339, "vy":0.61512, "omega":1.77947, "ax":-0.27769, "ay":2.65723, "alpha":0.09457, "fx":[-5.13873,-5.04098,-4.04109,-4.14391], "fy":[44.26054,43.48653,43.60617,44.37953]},
-    {"t":1.3788, "x":6.63785, "y":0.68479, "heading":0.15537, "vx":2.8784, "vy":0.66291, "omega":1.78117, "ax":-0.13383, "ay":2.23161, "alpha":0.05663, "fx":[-2.54194,-2.4583,-1.88264,-1.96772], "fy":[37.09617,36.61515,36.69631,37.17704]},
-    {"t":1.39678, "x":6.68959, "y":0.69707, "heading":0.18741, "vx":2.87599, "vy":0.70305, "omega":1.78219, "ax":-0.23962, "ay":2.36177, "alpha":0.14942, "fx":[-4.85198,-4.59676,-3.06627,-3.33221], "fy":[39.53904,38.29274,38.55865,39.80262]},
-    {"t":1.41477, "x":6.74128, "y":0.7101, "heading":0.21946, "vx":2.87168, "vy":0.74552, "omega":1.78488, "ax":-0.52287, "ay":2.90911, "alpha":0.20829, "fx":[-9.94037,-9.5673,-7.33571,-7.73612], "fy":[48.67383,47.05242,47.52441,49.13972]},
-    {"t":1.43275, "x":6.79284, "y":0.72397, "heading":0.25156, "vx":2.86228, "vy":0.79784, "omega":1.78862, "ax":-0.91067, "ay":3.60077, "alpha":0.25263, "fx":[-16.70509,-16.27879,-13.38076,-13.86123], "fy":[60.043,58.28983,59.02999,60.76933]},
-    {"t":1.45074, "x":6.84417, "y":0.73891, "heading":0.28373, "vx":2.8459, "vy":0.8626, "omega":1.79317, "ax":-1.33665, "ay":4.15741, "alpha":0.40371, "fx":[-24.80925,-24.25376,-19.30356,-20.03087], "fy":[69.22873,66.75267,68.27097,70.69304]},
-    {"t":1.46872, "x":6.89514, "y":0.75509, "heading":0.31598, "vx":2.82186, "vy":0.93737, "omega":1.80043, "ax":-1.77187, "ay":4.44946, "alpha":0.79616, "fx":[-34.57058,-33.90413,-23.64743,-25.05823], "fy":[74.01887,69.47737,73.25827,77.50502]},
-    {"t":1.48671, "x":6.9456, "y":0.77267, "heading":0.34836, "vx":2.78999, "vy":1.0174, "omega":1.81475, "ax":-2.23232, "ay":4.50551, "alpha":1.52362, "fx":[-46.40764,-46.30251,-26.02865,-28.89289], "fy":[74.70647,65.8961,74.97583,82.38783]},
-    {"t":1.50469, "x":6.99542, "y":0.7917, "heading":0.381, "vx":2.74985, "vy":1.09843, "omega":1.84215, "ax":-2.71901, "ay":4.41728, "alpha":2.49388, "fx":[-59.13543,-60.95606,-27.29462,-32.4324], "fy":[72.75124,57.12154,75.58211,86.67648]},
-    {"t":1.52268, "x":7.04443, "y":0.81217, "heading":0.41413, "vx":2.70095, "vy":1.17787, "omega":1.887, "ax":-3.17635, "ay":4.28363, "alpha":3.40553, "fx":[-70.27643,-74.55728,-28.84502,-36.38507], "fy":[69.79417,46.43933,76.6986,90.36063]},
-    {"t":1.54066, "x":7.0925, "y":0.83404, "heading":0.44806, "vx":2.64382, "vy":1.25491, "omega":1.94825, "ax":-3.55529, "ay":4.20878, "alpha":3.98752, "fx":[-78.43589,-84.44025,-31.55327,-40.69529], "fy":[67.39334,38.50136,79.2468,93.20116]},
-    {"t":1.55865, "x":7.13947, "y":0.85729, "heading":0.4831, "vx":2.57988, "vy":1.33061, "omega":2.01996, "ax":-3.84962, "ay":4.27077, "alpha":4.12607, "fx":[-83.51327,-90.4069,-35.61951,-45.05047], "fy":[66.7517,36.80268,83.54602,95.34186]},
-    {"t":1.57663, "x":7.18525, "y":0.88191, "heading":0.51943, "vx":2.51064, "vy":1.40741, "omega":2.09417, "ax":-4.0719, "ay":4.48336, "alpha":3.815, "fx":[-85.87546,-92.91436,-41.07203,-49.42834], "fy":[68.41144,42.48564,88.7181,96.88634]},
-    {"t":1.59462, "x":7.22974, "y":0.90795, "heading":0.55709, "vx":2.43741, "vy":1.48805, "omega":2.16278, "ax":-4.23674, "ay":4.78863, "alpha":3.12779, "fx":[-86.02184,-92.01047,-47.96506,-54.19391], "fy":[72.12773,53.94629,92.97593,97.63997]},
-    {"t":1.6126, "x":7.27289, "y":0.93549, "heading":0.59599, "vx":2.36121, "vy":1.57417, "omega":2.21904, "ax":-4.36782, "ay":5.08746, "alpha":2.22775, "fx":[-84.59258,-88.31641,-56.12888,-59.82229], "fy":[77.0619,67.20014,94.95403,97.23711]},
-    {"t":1.63059, "x":7.31465, "y":0.96462, "heading":0.6359, "vx":2.28266, "vy":1.66567, "omega":2.2591, "ax":-4.50554, "ay":5.29311, "alpha":1.30572, "fx":[-82.46244,-83.86876,-65.05591,-66.58104], "fy":[82.12192,78.28086,94.30665,95.34365]},
-    {"t":1.64857, "x":7.35498, "y":0.99543, "heading":0.67653, "vx":2.20163, "vy":1.76086, "omega":2.28258, "ax":-4.70752, "ay":5.35131, "alpha":0.47361, "fx":[-80.92291,-81.10256,-74.53312,-74.7672], "fy":[86.09158,85.28959,91.05669,91.46434]},
-    {"t":1.66656, "x":7.39381, "y":1.02797, "heading":0.71758, "vx":2.11696, "vy":1.8571, "omega":2.2911, "ax":-4.949, "ay":5.28656, "alpha":-0.14165, "fx":[-80.87353,-80.89623,-82.75591,-82.77067], "fy":[88.2235,88.3401,86.60515,86.45116]},
-    {"t":1.68454, "x":7.43108, "y":1.06222, "heading":0.75879, "vx":2.02796, "vy":1.95218, "omega":2.28855, "ax":-5.17321, "ay":5.16247, "alpha":-0.53263, "fx":[-81.94378,-82.2293,-88.85694,-89.09415], "fy":[88.73618,88.86341,82.24665,81.56766]},
-    {"t":1.70253, "x":7.46672, "y":1.09817, "heading":0.79995, "vx":1.93492, "vy":2.04503, "omega":2.27898, "ax":-5.38785, "ay":4.99607, "alpha":-0.81352, "fx":[-83.66957,-84.31997,-93.89805,-94.4314], "fy":[88.15379,88.00814,77.71782,76.52916]},
-    {"t":1.72051, "x":7.50065, "y":1.13575, "heading":0.84093, "vx":1.83802, "vy":2.13488, "omega":2.26434, "ax":-5.59756, "ay":4.7953, "alpha":-1.05058, "fx":[-85.73207,-86.77253,-98.41697,-99.26584], "fy":[86.89114,86.36148,72.82423,71.05439]},
-    {"t":1.7385, "x":7.5328, "y":1.17493, "heading":0.88166, "vx":1.73735, "vy":2.22112, "omega":2.24545, "ax":-5.7192, "ay":4.60473, "alpha":-1.28109, "fx":[-86.32952,-88.0619,-101.43605,-102.4046], "fy":[85.81165,84.73549,68.167,65.81411]},
-    {"t":1.75407, "x":7.55917, "y":1.21008, "heading":0.91664, "vx":1.64826, "vy":2.29285, "omega":2.22549, "ax":-5.7554, "ay":4.47398, "alpha":-1.74933, "fx":[-83.63396,-87.20246,-104.23781,-105.55177], "fy":[87.73828,85.26043,63.30291,59.57936]},
-    {"t":1.76965, "x":7.58414, "y":1.24634, "heading":0.9513, "vx":1.55861, "vy":2.36255, "omega":2.19824, "ax":-5.77097, "ay":4.34078, "alpha":-2.24071, "fx":[-80.28855,-86.42086,-106.68569,-108.261], "fy":[89.98041,85.63034,58.43865,53.02298]},
-    {"t":1.78523, "x":7.60772, "y":1.28367, "heading":0.98554, "vx":1.46871, "vy":2.43016, "omega":2.16334, "ax":-5.76295, "ay":4.20903, "alpha":-2.73947, "fx":[-76.34629,-85.72512,-108.70169,-110.35268], "fy":[92.37724,85.81949,53.74109,46.42109]},
-    {"t":1.8008, "x":7.6299, "y":1.32204, "heading":1.01924, "vx":1.37894, "vy":2.49573, "omega":2.12067, "ax":-5.73381, "ay":4.07683, "alpha":-3.22811, "fx":[-72.01647,-85.15868,-110.2824,-111.74059], "fy":[94.65197,85.75731,49.26033,39.94684]},
-    {"t":1.81638, "x":7.65069, "y":1.36141, "heading":1.05228, "vx":1.28962, "vy":2.55923, "omega":2.07038, "ax":-5.68955, "ay":3.93963, "alpha":-3.68093, "fx":[-67.69138,-84.78762,-111.43191,-112.36043], "fy":[96.44109,85.33387,45.00448,33.76288]},
-    {"t":1.83196, "x":7.67009, "y":1.40175, "heading":1.08453, "vx":1.201, "vy":2.6206, "omega":2.01304, "ax":-5.63999, "ay":3.78972, "alpha":-4.06233, "fx":[-63.97571,-84.70997,-112.1466,-112.1616], "fy":[97.30187,84.37452,40.93717,28.01504]},
-    {"t":1.84754, "x":7.68811, "y":1.44303, "heading":1.11589, "vx":1.11314, "vy":2.67964, "omega":1.94976, "ax":-5.59889, "ay":3.61495, "alpha":-4.32387, "fx":[-61.70717,-85.06991,-112.39555,-111.10292], "fy":[96.67,82.58967,36.97399,22.83663]},
-    {"t":1.86311, "x":7.70477, "y":1.48521, "heading":1.14626, "vx":1.02593, "vy":2.73595, "omega":1.88241, "ax":-5.58406, "ay":3.39534, "alpha":-4.39566, "fx":[-61.97906,-86.07169,-112.08483,-109.15919], "fy":[93.68701,79.47513,32.99029,18.39441]},
-    {"t":1.87869, "x":7.72007, "y":1.52824, "heading":1.17558, "vx":0.93894, "vy":2.78884, "omega":1.81394, "ax":-5.61532, "ay":3.09656, "alpha":-4.16911, "fx":[-66.06745,-87.9602,-110.98505,-106.3497], "fy":[86.80506,74.11739,28.86376,15.00107]},
-    {"t":1.89427, "x":7.73402, "y":1.57206, "heading":1.20384, "vx":0.85147, "vy":2.83707, "omega":1.74899, "ax":-5.69631, "ay":2.66525, "alpha":-3.49305, "fx":[-74.5936,-90.79085,-108.58074,-102.75351], "fy":[73.43815,64.91937,24.63486,13.27091]},
-    {"t":1.90985, "x":7.74659, "y":1.61658, "heading":1.23108, "vx":0.76274, "vy":2.87859, "omega":1.69458, "ax":-5.74559, "ay":2.07133, "alpha":-2.3186, "fx":[-84.40767,-93.37729,-103.8821,-98.31033], "fy":[52.36763,50.09922,20.78795,13.73036]},
-    {"t":1.92542, "x":7.75777, "y":1.66167, "heading":1.25748, "vx":0.67324, "vy":2.91085, "omega":1.65847, "ax":-5.55427, "ay":1.4558, "alpha":-1.09332, "fx":[-87.5577,-91.74467,-95.86975,-92.1529], "fy":[31.41177,32.40696,17.71508,14.74401]},
-    {"t":1.941, "x":7.76759, "y":1.70719, "heading":1.28331, "vx":0.58672, "vy":2.93353, "omega":1.64143, "ax":-5.02429, "ay":1.0154, "alpha":-0.34362, "fx":[-81.70945,-83.34227,-84.4128,-82.81085], "fy":[18.75314,19.42398,14.90431,14.07102]},
-    {"t":1.95658, "x":7.77612, "y":1.75301, "heading":1.30888, "vx":0.50845, "vy":2.94935, "omega":1.63608, "ax":-4.23569, "ay":0.71181, "alpha":0.10833, "fx":[-70.50204,-69.84741,-69.55796,-70.21498], "fy":[11.23283,10.98567,12.31049,12.54543]},
-    {"t":1.97215, "x":7.78352, "y":1.79904, "heading":1.33437, "vx":0.44247, "vy":2.96044, "omega":1.63777, "ax":-3.34075, "ay":0.48569, "alpha":0.49021, "fx":[-57.57471,-54.02563,-52.87398,-56.46217], "fy":[5.8796,4.75638,10.26974,11.21507]},
-    {"t":1.98773, "x":7.79001, "y":1.84521, "heading":1.35988, "vx":0.39043, "vy":2.968, "omega":1.64541, "ax":-2.57513, "ay":0.33187, "alpha":0.81614, "fx":[-46.6898,-40.11608,-38.42067,-45.07656], "fy":[2.13785,0.39132,9.01324,10.4054]},
-    {"t":2.00331, "x":7.79578, "y":1.89149, "heading":1.38551, "vx":0.35032, "vy":2.97317, "omega":1.65812, "ax":-2.1313, "ay":0.24728, "alpha":1.03395, "fx":[-40.52805,-31.78195,-29.89758,-38.74366], "fy":[-0.10799,-2.09621,8.51329,10.04424]},
-    {"t":2.01889, "x":7.80098, "y":1.93783, "heading":1.41134, "vx":0.31712, "vy":2.97702, "omega":1.67422, "ax":-2.05837, "ay":0.21496, "alpha":1.11587, "fx":[-39.663,-30.11238,-28.35279,-37.99991], "fy":[-1.09667,-2.98591,8.46239,9.83649]},
-    {"t":2.03446, "x":7.80567, "y":1.98423, "heading":1.43742, "vx":0.28506, "vy":2.98037, "omega":1.69161, "ax":-2.29765, "ay":0.21206, "alpha":1.06022, "fx":[-43.14498,-34.20969,-32.79159,-41.80595], "fy":[-1.14738,-2.69451,8.42519,9.44118]},
-    {"t":2.05004, "x":7.80983, "y":2.03068, "heading":1.46377, "vx":0.24926, "vy":2.98368, "omega":1.70812, "ax":-2.70557, "ay":0.21404, "alpha":0.86947, "fx":[-48.73143,-41.66424,-40.70914,-47.82492], "fy":[-0.55221,-1.59197,7.8479,8.45125]},
-    {"t":2.06562, "x":7.81339, "y":2.07718, "heading":1.49038, "vx":0.20712, "vy":2.98701, "omega":1.72167, "ax":-2.99185, "ay":0.19487, "alpha":0.40746, "fx":[-51.24474,-48.02555,-47.68208,-50.91002], "fy":[1.16827,0.83522,5.33014,5.55419]},
-    {"t":2.08119, "x":7.81625, "y":2.12374, "heading":1.5172, "vx":0.16051, "vy":2.99005, "omega":1.72801, "ax":-2.35919, "ay":0.16028, "alpha":-0.9647, "fx":[-34.62273,-42.8272,-43.37712,-35.19513], "fy":[7.54974,7.73508,-2.02072,-2.66388]},
-    {"t":2.09677, "x":7.81846, "y":2.17033, "heading":1.54411, "vx":0.12377, "vy":2.99254, "omega":1.71299, "ax":-0.20738, "ay":-0.02151, "alpha":-7.68661, "fx":[33.84422,-38.77265,-40.51961,31.7334], "fy":[35.5317,36.31773,-35.08814,-38.1841]},
-    {"t":2.11235, "x":7.82037, "y":2.21695, "heading":1.5708, "vx":0.12053, "vy":2.99221, "omega":1.59325, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":2.51068, "y":0.66066, "heading":0.0, "vx":1.99816, "vy":-0.00825, "omega":0.00422, "ax":0.03108, "ay":-0.00432, "alpha":-0.0018, "fx":[0.52241,0.52241,0.50544,0.50544], "fy":[-0.07998,-0.063,-0.063,-0.07998]},
+    {"t":0.04394, "x":2.59851, "y":0.66029, "heading":0.00019, "vx":1.99953, "vy":-0.00844, "omega":0.00414, "ax":-0.00003, "ay":-0.00793, "alpha":-0.00371, "fx":[0.01694,0.01694,-0.01807,-0.01806], "fy":[-0.14865,-0.11365,-0.11365,-0.14866]},
+    {"t":0.08788, "x":2.68638, "y":0.65992, "heading":0.00037, "vx":1.99953, "vy":-0.00878, "omega":0.00397, "ax":-0.00003, "ay":-0.00686, "alpha":-0.00585, "fx":[0.02711,0.02709,-0.02813,-0.02811], "fy":[-0.14107,-0.08585,-0.08587,-0.14109]},
+    {"t":0.13182, "x":2.77424, "y":0.65952, "heading":0.00054, "vx":1.99952, "vy":-0.00909, "omega":0.00372, "ax":-0.00003, "ay":-0.00573, "alpha":-0.00835, "fx":[0.03895,0.03891,-0.03983,-0.03979], "fy":[-0.13416,-0.05542,-0.05547,-0.1342]},
+    {"t":0.17576, "x":2.8621, "y":0.65912, "heading":0.0007, "vx":1.99952, "vy":-0.00934, "omega":0.00335, "ax":-0.00002, "ay":-0.00479, "alpha":-0.01131, "fx":[0.053,0.05292,-0.05375,-0.05368], "fy":[-0.13253,-0.02585,-0.02593,-0.1326]},
+    {"t":0.21971, "x":2.94996, "y":0.6587, "heading":0.00085, "vx":1.99952, "vy":-0.00955, "omega":0.00285, "ax":-0.00002, "ay":-0.00402, "alpha":-0.01486, "fx":[0.06983,0.06971,-0.07048,-0.07036], "fy":[-0.13647,0.00371,0.00359,-0.13659]},
+    {"t":0.26365, "x":3.03782, "y":0.65828, "heading":0.00098, "vx":1.99952, "vy":-0.00973, "omega":0.0022, "ax":-0.00002, "ay":-0.00339, "alpha":-0.01912, "fx":[0.09003,0.08985,-0.09059,-0.09041], "fy":[-0.14615,0.03429,0.03412,-0.14632]},
+    {"t":0.30759, "x":3.12568, "y":0.65785, "heading":0.00107, "vx":1.99952, "vy":-0.00987, "omega":0.00136, "ax":-0.00001, "ay":-0.00288, "alpha":-0.02423, "fx":[0.11418,0.11394,-0.11466,-0.11442], "fy":[-0.16177,0.06683,0.06659,-0.16201]},
+    {"t":0.35153, "x":3.21354, "y":0.65741, "heading":0.00113, "vx":1.99952, "vy":-0.01, "omega":0.0003, "ax":-0.00001, "ay":-0.00247, "alpha":-0.03029, "fx":[0.14287,0.14255,-0.14329,-0.14296], "fy":[-0.18362,0.10221,0.10189,-0.18394]},
+    {"t":0.39547, "x":3.30141, "y":0.65697, "heading":0.00115, "vx":1.99952, "vy":-0.01011, "omega":-0.00104, "ax":-0.00001, "ay":-0.00215, "alpha":-0.03744, "fx":[0.17665,0.17624,-0.17702,-0.17661], "fy":[-0.21203,0.14123,0.14082,-0.21243]},
+    {"t":0.43941, "x":3.38927, "y":0.65652, "heading":0.0011, "vx":1.99952, "vy":-0.0102, "omega":-0.00268, "ax":-0.00001, "ay":-0.00191, "alpha":-0.04578, "fx":[0.21604,0.21556,-0.21637,-0.21589], "fy":[-0.24734,0.18459,0.18411,-0.24782]},
+    {"t":0.48335, "x":3.47713, "y":0.65607, "heading":0.00098, "vx":1.99952, "vy":-0.01029, "omega":-0.00469, "ax":-0.00001, "ay":-0.00174, "alpha":-0.05541, "fx":[0.2615,0.26098,-0.2618,-0.26129], "fy":[-0.2899,0.23289,0.23238,-0.29041]},
+    {"t":0.52729, "x":3.56499, "y":0.65562, "heading":0.00078, "vx":1.99952, "vy":-0.01036, "omega":-0.00713, "ax":-0.00001, "ay":-0.00163, "alpha":-0.06641, "fx":[0.3134,0.31292,-0.31369,-0.3132], "fy":[-0.34,0.28661,0.28612,-0.34049]},
+    {"t":0.57124, "x":3.65285, "y":0.65516, "heading":0.00046, "vx":1.99952, "vy":-0.01044, "omega":-0.01005, "ax":-0.00001, "ay":-0.00158, "alpha":-0.07885, "fx":[0.37199,0.37164,-0.37227,-0.37192], "fy":[-0.39787,0.34604,0.3457,-0.39822]},
+    {"t":0.61518, "x":3.74071, "y":0.6547, "heading":0.00002, "vx":1.99952, "vy":-0.01051, "omega":-0.01351, "ax":-0.00001, "ay":-0.00158, "alpha":-0.09273, "fx":[0.43731,0.43729,-0.43759,-0.43757], "fy":[-0.4636,0.41128,0.41126,-0.46362]},
+    {"t":0.65912, "x":3.82857, "y":0.65424, "heading":-0.00057, "vx":1.99952, "vy":-0.01057, "omega":-0.01758, "ax":-0.00001, "ay":-0.00165, "alpha":-0.10803, "fx":[0.50919,0.50977,-0.50948,-0.51006], "fy":[-0.53712,0.48214,0.48272,-0.53654]},
+    {"t":0.70306, "x":3.91643, "y":0.65377, "heading":-0.00134, "vx":1.99952, "vy":-0.01065, "omega":-0.02233, "ax":-0.00001, "ay":-0.00177, "alpha":-0.12466, "fx":[0.58713,0.58872,-0.58745,-0.58903], "fy":[-0.61808,0.55809,0.55967,-0.6165]},
+    {"t":0.747, "x":4.0043, "y":0.6533, "heading":-0.00232, "vx":1.99952, "vy":-0.01072, "omega":-0.02781, "ax":-0.00001, "ay":-0.00195, "alpha":-0.14245, "fx":[0.67027,0.67339,-0.67062,-0.67374], "fy":[-0.70583,0.63818,0.6413,-0.70271]},
+    {"t":0.79094, "x":4.09216, "y":0.65283, "heading":-0.00355, "vx":1.99952, "vy":-0.01081, "omega":-0.03407, "ax":-0.00001, "ay":-0.00221, "alpha":-0.16113, "fx":[0.75722,0.76262,-0.75763,-0.76302], "fy":[-0.7993,0.72094,0.72633,-0.79391]},
+    {"t":0.83488, "x":4.18002, "y":0.65235, "heading":-0.00504, "vx":1.99951, "vy":-0.01091, "omega":-0.04115, "ax":-0.00001, "ay":-0.00254, "alpha":-0.1803, "fx":[0.84604,0.85463,-0.84651,-0.85509], "fy":[-0.89687,0.80426,0.81284,-0.88829]},
+    {"t":0.87882, "x":4.26788, "y":0.65187, "heading":-0.00685, "vx":1.99951, "vy":-0.01102, "omega":-0.04907, "ax":-0.00002, "ay":-0.00297, "alpha":-0.19943, "fx":[0.93405,0.94696,-0.93461,-0.9475], "fy":[-0.99628,0.88528,0.89817,-0.98339]},
+    {"t":0.92277, "x":4.35574, "y":0.65139, "heading":-0.00901, "vx":1.99951, "vy":-0.01115, "omega":-0.05783, "ax":-0.00002, "ay":-0.0035, "alpha":-0.21778, "fx":[1.01773,1.03626,-1.0184,-1.0369], "fy":[-1.09444,0.96021,0.97872,-1.07593]},
+    {"t":0.96671, "x":4.4436, "y":0.65089, "heading":-0.01155, "vx":1.99951, "vy":-0.0113, "omega":-0.0674, "ax":-0.00002, "ay":-0.00416, "alpha":-0.23441, "fx":[1.0926,1.11817,-1.09341,-1.11894], "fy":[-1.1873,1.02426,1.04981,-1.16175]},
+    {"t":1.01065, "x":4.53146, "y":0.65039, "heading":-0.01451, "vx":1.99951, "vy":-0.01149, "omega":-0.0777, "ax":-0.00003, "ay":-0.00497, "alpha":-0.24814, "fx":[1.15299,1.18699,-1.15397,-1.18792], "fy":[-1.26955,1.07138,1.10536,-1.23558]},
+    {"t":1.05459, "x":4.61932, "y":0.64988, "heading":-0.01793, "vx":1.99951, "vy":-0.0117, "omega":-0.08861, "ax":-0.00004, "ay":-0.00595, "alpha":-0.25747, "fx":[1.19204,1.23562,-1.19325,-1.23676], "fy":[-1.33458,1.09425,1.1378,-1.29104]},
+    {"t":1.09853, "x":4.70718, "y":0.64936, "heading":-0.02182, "vx":1.99951, "vy":-0.01197, "omega":-0.09992, "ax":-0.00004, "ay":-0.00715, "alpha":-0.26055, "fx":[1.20131,1.25499,-1.20279,-1.25639], "fy":[-1.3739,1.08383,1.13747,-1.32027]},
+    {"t":1.14247, "x":4.79505, "y":0.64883, "heading":-0.02621, "vx":1.99951, "vy":-0.01228, "omega":-0.11137, "ax":-0.00005, "ay":-0.00861, "alpha":-0.2552, "fx":[1.17104,1.2342,-1.17287,-1.23592], "fy":[-1.37733,1.02967,1.09278,-1.31423]},
+    {"t":1.18641, "x":4.88291, "y":0.64828, "heading":-0.0311, "vx":1.9995, "vy":-0.01266, "omega":-0.12258, "ax":-0.00007, "ay":-0.01037, "alpha":-0.2387, "fx":[1.08937,1.15947,-1.09164,-1.16163], "fy":[-1.33198,0.91906,0.98911,-1.26194]},
+    {"t":1.23035, "x":4.97077, "y":0.64771, "heading":-0.03649, "vx":1.9995, "vy":-0.01311, "omega":-0.13307, "ax":-0.00008, "ay":-0.01251, "alpha":-0.20796, "fx":[0.94319,1.01483,-0.94602,-1.01755], "fy":[-1.22294,0.73784,0.80942,-1.15136]},
+    {"t":1.2743, "x":5.05863, "y":0.64713, "heading":-0.04234, "vx":1.9995, "vy":-0.01366, "omega":-0.14221, "ax":-0.00011, "ay":-0.01509, "alpha":-0.15918, "fx":[0.71669,0.7803,-0.72023,-0.78376], "fy":[-1.03157,0.46891,0.53248,-0.968]},
+    {"t":1.31824, "x":5.14649, "y":0.64651, "heading":-0.04859, "vx":1.99949, "vy":-0.01433, "omega":-0.14921, "ax":-0.00013, "ay":-0.01823, "alpha":-0.08818, "fx":[0.39309,0.43351,-0.39754,-0.43794], "fy":[-0.73714,0.09389,0.1343,-0.69673]},
+    {"t":1.36218, "x":5.23435, "y":0.64586, "heading":-0.05514, "vx":1.99949, "vy":-0.01513, "omega":-0.15308, "ax":-0.00017, "ay":-0.02204, "alpha":0.01011, "fx":[-0.04783,-0.05309,0.04214,0.0474], "fy":[-0.31419,-0.40942,-0.41468,-0.31945]},
+    {"t":1.40612, "x":5.32221, "y":0.64518, "heading":-0.06187, "vx":1.99948, "vy":-0.0161, "omega":-0.15264, "ax":-0.00022, "ay":-0.02669, "alpha":0.14107, "fx":[-0.6268,-0.70905,0.61938,0.70174], "fy":[0.2641,-1.06435,-1.14665,0.1818]},
+    {"t":1.45006, "x":5.41007, "y":0.64445, "heading":-0.06858, "vx":1.99947, "vy":-0.01727, "omega":-0.14644, "ax":-0.00029, "ay":-0.03239, "alpha":0.31113, "fx":[-1.36872,-1.56958,1.35879,1.56025], "fy":[1.02932,-1.89928,-2.1004,0.82813]},
+    {"t":1.494, "x":5.49793, "y":0.64366, "heading":-0.07501, "vx":1.99946, "vy":-0.01869, "omega":-0.13277, "ax":-0.00039, "ay":-0.03946, "alpha":0.52677, "fx":[-2.29879,-2.67025,2.285,2.65851], "fy":[2.0119,-2.94425,-3.31657,1.63929]},
+    {"t":1.53794, "x":5.58578, "y":0.6428, "heading":-0.08084, "vx":1.99944, "vy":-0.02043, "omega":-0.10962, "ax":-0.00052, "ay":-0.04836, "alpha":0.79533, "fx":[-3.44691,-4.05007,3.42685,4.03575], "fy":[3.24323,-4.23638,-4.84194,2.6368]},
+    {"t":1.58188, "x":5.67364, "y":0.64185, "heading":-0.08566, "vx":1.99942, "vy":-0.02255, "omega":-0.07467, "ax":-0.00071, "ay":-0.05986, "alpha":1.12374, "fx":[-4.84393,-5.74403,4.81322,5.72748], "fy":[4.74591,-5.81822,-6.72418,3.83767]},
+    {"t":1.62583, "x":5.7615, "y":0.6408, "heading":-0.08894, "vx":1.99939, "vy":-0.02518, "omega":-0.02529, "ax":-0.00101, "ay":-0.07535, "alpha":1.5187, "fx":[-6.52533,-7.78196,6.47562,7.76479], "fy":[6.52748,-7.74619,-9.01622,5.25205]},
+    {"t":1.66977, "x":5.84935, "y":0.63962, "heading":-0.09005, "vx":1.99934, "vy":-0.02849, "omega":0.04144, "ax":-0.0015, "ay":-0.09769, "alpha":1.9855, "fx":[-8.53063,-10.1795,8.44506,10.16606], "fy":[8.55723,-10.10303,-11.78163,6.86663]},
+    {"t":1.71371, "x":5.9372, "y":0.63828, "heading":-0.08823, "vx":1.99928, "vy":-0.03278, "omega":0.12869, "ax":-0.00239, "ay":-0.13402, "alpha":2.52665, "fx":[-10.9062,-12.92702,10.7468,12.92804], "fy":[10.71017,-13.04131,-15.12887,8.59697]},
+    {"t":1.75765, "x":6.02505, "y":0.63671, "heading":-0.08258, "vx":1.99917, "vy":-0.03867, "omega":0.23971, "ax":-0.00438, "ay":-0.20296, "alpha":3.14049, "fx":[-13.72009,-15.97654,13.38707,16.01999], "fy":[12.63353,-16.90129,-19.31766,10.16285]},
+    {"t":1.80159, "x":6.11289, "y":0.63481, "heading":-0.07205, "vx":1.99898, "vy":-0.04759, "omega":0.37771, "ax":-0.0075, "ay":-0.279, "alpha":3.82054, "fx":[-17.02119,-19.2318,16.38889,19.36829], "fy":[14.65802,-21.28899,-23.83848,12.01834]},
+    {"t":1.84553, "x":6.20072, "y":0.63245, "heading":-0.05545, "vx":1.99865, "vy":-0.05985, "omega":0.54559, "ax":0.0123, "ay":2.21564, "alpha":3.88477, "fx":[-16.97284,-22.49263,20.77844,19.50078], "fy":[54.12255,21.06211,18.87986,52.46394]},
+    {"t":1.88947, "x":6.28856, "y":0.63196, "heading":-0.03147, "vx":1.99919, "vy":0.03751, "omega":0.71629, "ax":-0.68378, "ay":6.94936, "alpha":0.43828, "fx":[-14.52486,-15.67827,-7.86096,-7.15696], "fy":[114.9212,114.19955,114.92013,115.54631]},
+    {"t":1.93341, "x":6.37575, "y":0.64032, "heading":0.0, "vx":1.96914, "vy":0.34287, "omega":0.73555, "ax":-1.46066, "ay":6.79946, "alpha":0.23939, "fx":[-25.73177,-26.58148,-22.5079,-21.77797], "fy":[112.28735,111.75858,112.57461,113.05311]},
+    {"t":1.95722, "x":6.42222, "y":0.65041, "heading":0.01751, "vx":1.93436, "vy":0.50477, "omega":0.74125, "ax":-1.7773, "ay":5.91401, "alpha":0.55802, "fx":[-32.49255,-34.52673,-26.04119,-24.47889], "fy":[97.98053,95.58384,97.69559,99.85607]},
+    {"t":1.98104, "x":6.46777, "y":0.6641, "heading":0.03516, "vx":1.89205, "vy":0.64558, "omega":0.75453, "ax":-1.3465, "ay":3.66564, "alpha":1.54031, "fx":[-29.686,-32.43893,-13.94129,-12.98286], "fy":[64.94212,53.33981,56.42583,67.71499]},
+    {"t":2.00485, "x":6.51244, "y":0.68051, "heading":0.05313, "vx":1.85999, "vy":0.73286, "omega":0.79121, "ax":-0.47244, "ay":1.17314, "alpha":2.37397, "fx":[-19.30941,-19.16619,4.33895,2.89222], "fy":[29.46891,7.73688,9.17473,31.20351]},
+    {"t":2.02866, "x":6.55659, "y":0.6983, "heading":0.07197, "vx":1.84874, "vy":0.76079, "omega":0.84773, "ax":-0.11777, "ay":0.28469, "alpha":2.36716, "fx":[-13.81172,-12.38833,10.07635,8.3352], "fy":[14.99606,-7.21137,-5.62199,16.665]},
+    {"t":2.05247, "x":6.60058, "y":0.71649, "heading":0.09215, "vx":1.84593, "vy":0.76757, "omega":0.9041, "ax":-0.02496, "ay":0.05995, "alpha":2.24642, "fx":[-11.92544,-10.00685,11.13058,9.15116], "fy":[10.56378,-10.53308,-8.58799,12.52184]},
+    {"t":2.07628, "x":6.64452, "y":0.73479, "heading":0.11368, "vx":1.84534, "vy":0.769, "omega":0.95758, "ax":-0.00213, "ay":0.00509, "alpha":2.12406, "fx":[-11.12633,-8.85561,11.05838,8.78301], "fy":[8.90275,-11.00785,-8.73479,11.17662]},
+    {"t":2.10009, "x":6.68846, "y":0.7531, "heading":0.13648, "vx":1.84529, "vy":0.76912, "omega":1.00816, "ax":0.00331, "ay":-0.00794, "alpha":2.00629, "fx":[-10.61143,-8.0328,10.71751,8.14542], "fy":[7.95777,-10.79595,-8.22002,10.53291]},
+    {"t":2.1239, "x":6.7324, "y":0.77141, "heading":0.16048, "vx":1.84537, "vy":0.76893, "omega":1.05593, "ax":0.00447, "ay":-0.01075, "alpha":1.89295, "fx":[-10.1703,-7.31241,10.31428,7.46432], "fy":[7.21076,-10.4201,-7.56582,10.06445]},
+    {"t":2.14771, "x":6.77634, "y":0.78971, "heading":0.18563, "vx":1.84547, "vy":0.76868, "omega":1.101, "ax":0.00461, "ay":-0.01108, "alpha":1.78366, "fx":[-9.74852,-6.63892,9.89732,6.795], "fy":[6.53384,-10.00611,-6.90005,9.63973]},
+    {"t":2.17152, "x":6.82028, "y":0.80801, "heading":0.21184, "vx":1.84558, "vy":0.76841, "omega":1.14347, "ax":0.00451, "ay":-0.01083, "alpha":1.67814, "fx":[-9.33132,-5.99907,9.47718,6.15123], "fy":[5.89602,-9.58331,-6.2543,9.22521]},
+    {"t":2.19533, "x":6.86423, "y":0.8263, "heading":0.23907, "vx":1.84569, "vy":0.76815, "omega":1.18343, "ax":0.00435, "ay":-0.01047, "alpha":1.57615, "fx":[-8.91415,-5.39005,9.05543,5.5367], "fy":[5.29019,-9.15778,-5.63661,8.81181]},
+    {"t":2.21914, "x":6.90817, "y":0.84459, "heading":0.26724, "vx":1.84579, "vy":0.7679, "omega":1.22096, "ax":0.0042, "ay":-0.01009, "alpha":1.47752, "fx":[-8.49525,-4.81166,8.63172,4.95264], "fy":[4.71517,-8.7302,-5.0492,8.3968]},
+    {"t":2.24295, "x":6.95212, "y":0.86287, "heading":0.29632, "vx":1.84589, "vy":0.76766, "omega":1.25614, "ax":0.00403, "ay":-0.0097, "alpha":1.38216, "fx":[-8.07448,-4.2647,8.20585,4.39985], "fy":[4.17178,-8.30033,-4.49284,7.98003]},
+    {"t":2.26676, "x":6.99608, "y":0.88115, "heading":0.32623, "vx":1.84599, "vy":0.76743, "omega":1.28905, "ax":0.00382, "ay":-0.00919, "alpha":1.28995, "fx":[-7.65228,-3.7504,7.77693,3.8781], "fy":[3.66221,-7.86628,-3.96635,7.56295]},
+    {"t":2.29057, "x":7.04003, "y":0.89942, "heading":0.35692, "vx":1.84608, "vy":0.76721, "omega":1.31976, "ax":0.00337, "ay":-0.00812, "alpha":1.20093, "fx":[-7.23264,-3.27264,7.34296,3.38529], "fy":[3.19451,-7.42184,-3.46338,7.15373]},
+    {"t":2.31438, "x":7.08399, "y":0.91768, "heading":0.38834, "vx":1.84616, "vy":0.76702, "omega":1.34835, "ax":0.00195, "ay":-0.0047, "alpha":1.115, "fx":[-6.82817,-2.84418,6.89211,2.90925], "fy":[2.79873,-6.93768,-2.9544,6.78246]},
+    {"t":2.33819, "x":7.12795, "y":0.93595, "heading":0.42045, "vx":1.84621, "vy":0.76691, "omega":1.3749, "ax":-0.00354, "ay":0.00851, "alpha":1.0323, "fx":[-6.4917,-2.51718,6.3755,2.39934], "fy":[2.59871,-6.29286,-2.31683,6.57394]},
+    {"t":2.362, "x":7.1719, "y":0.95421, "heading":0.45318, "vx":1.84612, "vy":0.76711, "omega":1.39948, "ax":-0.02592, "ay":0.06231, "alpha":0.95257, "fx":[-6.43414,-2.50468,5.58205,1.64244], "fy":[3.10324,-4.97857,-1.04008,7.03605]},
+    {"t":2.38581, "x":7.21585, "y":0.97249, "heading":0.4865, "vx":1.84551, "vy":0.7686, "omega":1.42216, "ax":-0.11869, "ay":0.28353, "alpha":0.87369, "fx":[-7.52814,-3.69719,3.62202,-0.24633], "fy":[6.40084,-0.88441,2.98661,10.24758]},
+    {"t":2.40962, "x":7.25976, "y":0.99087, "heading":0.52037, "vx":1.84268, "vy":0.77535, "omega":1.44297, "ax":-0.4996, "ay":1.16267, "alpha":0.75959, "fx":[-13.18956,-9.74275,-3.27259,-6.83543], "fy":[20.44439,14.37958,18.04222,24.02572]},
+    {"t":2.43343, "x":7.30349, "y":1.00966, "heading":0.55472, "vx":1.83078, "vy":0.80303, "omega":1.46105, "ax":-1.65233, "ay":3.54027, "alpha":0.42303, "fx":[-30.37157,-28.6783,-24.1936,-26.03111], "fy":[58.5197,56.10743,58.58507,60.9195]},
+    {"t":2.45724, "x":7.34662, "y":1.02979, "heading":0.58951, "vx":1.79144, "vy":0.88732, "omega":1.47112, "ax":-2.94074, "ay":5.42487, "alpha":0.13105, "fx":[-49.68893,-49.3426,-47.53708,-47.91328], "fy":[89.32104,89.04902,90.06961,90.32744]},
+    {"t":2.48105, "x":7.38844, "y":1.05245, "heading":0.62454, "vx":1.72142, "vy":1.01649, "omega":1.47424, "ax":-3.78071, "ay":5.83419, "alpha":0.04771, "fx":[-62.89086,-62.82233,-62.12308,-62.19637], "fy":[96.24894,96.21402,96.67096,96.70306]},
+    {"t":2.50486, "x":7.42835, "y":1.07831, "heading":0.65964, "vx":1.6314, "vy":1.15541, "omega":1.47538, "ax":-4.42969, "ay":5.71279, "alpha":0.02162, "fx":[-73.39755,-73.38812,-73.078,-73.0884], "fy":[94.33864,94.32553,94.566,94.57835]},
+    {"t":2.52867, "x":7.46594, "y":1.10744, "heading":0.69477, "vx":1.52593, "vy":1.29143, "omega":1.47589, "ax":-4.99124, "ay":5.39459, "alpha":0.01119, "fx":[-82.59573,-82.60023,-82.44883,-82.44456], "fy":[89.12689,89.11579,89.25552,89.26638]},
+    {"t":2.55248, "x":7.50086, "y":1.13972, "heading":0.72991, "vx":1.40709, "vy":1.41988, "omega":1.47616, "ax":-5.52248, "ay":4.94207, "alpha":0.00625, "fx":[-91.34046,-91.34764,-91.2706,-91.26348], "fy":[81.67193,81.66107,81.74685,81.75763]},
+    {"t":2.57629, "x":7.5328, "y":1.17493, "heading":0.76506, "vx":1.2756, "vy":1.53755, "omega":1.47631, "ax":-5.85253, "ay":4.53184, "alpha":0.00558, "fx":[-96.78988,-96.79774,-96.73487,-96.72705], "fy":[74.89309,74.87999,74.96077,74.97379]},
+    {"t":2.59809, "x":7.55921, "y":1.20952, "heading":0.79724, "vx":1.14802, "vy":1.63633, "omega":1.47643, "ax":-6.17421, "ay":3.97705, "alpha":0.00727, "fx":[-102.11088,-102.1226,-102.05098,-102.03931], "fy":[65.7116,65.68723,65.79719,65.82143]},
+    {"t":2.61989, "x":7.58277, "y":1.24613, "heading":0.82943, "vx":1.01344, "vy":1.72303, "omega":1.47659, "ax":-6.37928, "ay":3.42323, "alpha":0.01016, "fx":[-105.50687,-105.52213,-105.43598,-105.42078], "fy":[56.54111,56.4976,56.6544,56.69766]},
+    {"t":2.64169, "x":7.60335, "y":1.2845, "heading":0.86161, "vx":0.87438, "vy":1.79765, "omega":1.47681, "ax":-6.431, "ay":2.83041, "alpha":0.01573, "fx":[-106.37476,-106.39035,-106.27821,-106.26269], "fy":[46.7167,46.63525,46.87632,46.95723]},
+    {"t":2.66348, "x":7.62088, "y":1.32436, "heading":0.8938, "vx":0.73419, "vy":1.85934, "omega":1.47715, "ax":-6.14306, "ay":2.17764, "alpha":0.02837, "fx":[-101.65438,-101.64616,-101.47755,-101.48587], "fy":[35.88314,35.71561,36.12536,36.29148]},
+    {"t":2.68528, "x":7.63542, "y":1.36541, "heading":0.926, "vx":0.60029, "vy":1.90681, "omega":1.47777, "ax":-4.95643, "ay":1.40898, "alpha":0.0614, "fx":[-82.20505,-82.0124,-81.68838,-81.88187], "fy":[23.11625,22.7415,23.47642,23.84724]},
+    {"t":2.70708, "x":7.64733, "y":1.40731, "heading":0.95822, "vx":0.49225, "vy":1.93753, "omega":1.47911, "ax":-2.49175, "ay":0.59622, "alpha":0.11293, "fx":[-41.86863,-41.12086,-40.52378,-41.27552], "fy":[9.67692,9.04464,10.04047,10.66853]},
+    {"t":2.72888, "x":7.65747, "y":1.44969, "heading":0.99046, "vx":0.43793, "vy":1.95052, "omega":1.48157, "ax":-0.79084, "ay":0.17391, "alpha":0.11399, "fx":[-13.81244,-12.92614,-12.33745,-13.22532], "fy":[2.71548,2.12334,3.03567,3.6266]},
+    {"t":2.75068, "x":7.66683, "y":1.49224, "heading":1.02275, "vx":0.42069, "vy":1.95431, "omega":1.48406, "ax":-0.22463, "ay":0.04806, "alpha":0.09143, "fx":[-4.30622,-3.57086,-3.12133,-3.85697], "fy":[0.65083,0.20114,0.93842,1.38788]},
+    {"t":2.77248, "x":7.67594, "y":1.53486, "heading":1.0551, "vx":0.41579, "vy":1.95536, "omega":1.48605, "ax":-0.06269, "ay":0.01331, "alpha":0.06721, "fx":[-1.46857,-0.91702,-0.6043,-1.15588], "fy":[0.10048,-0.21225,0.33949,0.65218]},
+    {"t":2.79427, "x":7.68499, "y":1.57748, "heading":1.0875, "vx":0.41443, "vy":1.95565, "omega":1.48752, "ax":-0.01708, "ay":0.00362, "alpha":0.04322, "fx":[-0.55762,-0.19658,-0.00709,-0.36814], "fy":[-0.02602,-0.21551,0.14556,0.33505]},
+    {"t":2.81607, "x":7.69402, "y":1.62011, "heading":1.11992, "vx":0.41406, "vy":1.95573, "omega":1.48846, "ax":-0.00427, "ay":0.0009, "alpha":0.01954, "fx":[-0.19373,-0.02783,0.0525,-0.11341], "fy":[-0.02788,-0.1082,0.05771,0.13803]},
+    {"t":2.83787, "x":7.70305, "y":1.66275, "heading":1.15237, "vx":0.41396, "vy":1.95575, "omega":1.48888, "ax":-0.00069, "ay":0.00015, "alpha":-0.00404, "fx":[0.01369,-0.02116,-0.03666,-0.00181], "fy":[0.01207,0.02757,-0.00728,-0.02278]},
+    {"t":2.85967, "x":7.71207, "y":1.70538, "heading":1.18482, "vx":0.41395, "vy":1.95575, "omega":1.4888, "ax":0.00029, "ay":-0.00006, "alpha":-0.02761, "fx":[0.17445,-0.06688,-0.16494,0.07638], "fy":[0.07059,0.16866,-0.07267,-0.17073]},
+    {"t":2.88147, "x":7.72109, "y":1.74801, "heading":1.21728, "vx":0.41395, "vy":1.95575, "omega":1.48819, "ax":0.00054, "ay":-0.00012, "alpha":-0.05134, "fx":[0.32002,-0.13442,-0.30212,0.15232], "fy":[0.14145,0.30915,-0.1453,-0.313]},
+    {"t":2.90326, "x":7.73012, "y":1.79064, "heading":1.24972, "vx":0.41397, "vy":1.95575, "omega":1.48707, "ax":0.00059, "ay":-0.00013, "alpha":-0.07534, "fx":[0.45922,-0.21531,-0.43965,0.23488], "fy":[0.22299,0.44733,-0.22719,-0.45153]},
+    {"t":2.92506, "x":7.73914, "y":1.83327, "heading":1.28213, "vx":0.41398, "vy":1.95575, "omega":1.48543, "ax":0.00059, "ay":-0.00013, "alpha":-0.09977, "fx":[0.59487,-0.3075,-0.57547,0.3269], "fy":[0.31512,0.58309,-0.31928,-0.58725]},
+    {"t":2.94686, "x":7.74817, "y":1.8759, "heading":1.31451, "vx":0.41399, "vy":1.95574, "omega":1.48326, "ax":0.00057, "ay":-0.00012, "alpha":-0.12473, "fx":[0.72775,-0.41068,-0.70901,0.42943], "fy":[0.41804,0.71637,-0.42206,-0.72039]},
+    {"t":2.96866, "x":7.75719, "y":1.91854, "heading":1.34684, "vx":0.414, "vy":1.95574, "omega":1.48054, "ax":0.00054, "ay":-0.00012, "alpha":-0.15039, "fx":[0.85829,-0.52518,-0.8403,0.54317], "fy":[0.53224,0.84736,-0.5361,-0.85122]},
+    {"t":2.99046, "x":7.76621, "y":1.96117, "heading":1.37912, "vx":0.41402, "vy":1.95574, "omega":1.47726, "ax":0.00052, "ay":-0.00011, "alpha":-0.17685, "fx":[0.98658,-0.65144,-0.96933,0.6687], "fy":[0.65822,0.9761,-0.66192,-0.9798]},
+    {"t":3.01226, "x":7.77524, "y":2.0038, "heading":1.41132, "vx":0.41403, "vy":1.95574, "omega":1.47341, "ax":0.0005, "ay":-0.00011, "alpha":-0.20427, "fx":[1.1127,-0.79013,-1.09619,0.80664], "fy":[0.79661,1.10267,-0.80015,-1.10621]},
+    {"t":3.03405, "x":7.78426, "y":2.04643, "heading":1.44344, "vx":0.41404, "vy":1.95573, "omega":1.46895, "ax":0.00047, "ay":-0.0001, "alpha":-0.23278, "fx":[1.2366,-0.94193,-1.2209,0.95763], "fy":[0.94809,1.22707,-0.95146,-1.23043]},
+    {"t":3.05585, "x":7.79329, "y":2.08906, "heading":1.47546, "vx":0.41405, "vy":1.95573, "omega":1.46388, "ax":0.00044, "ay":-0.00009, "alpha":-0.26255, "fx":[1.3581,-1.10779,-1.3436,1.12228], "fy":[1.11348,1.3493,-1.11659,-1.3524]},
+    {"t":3.07765, "x":7.80232, "y":2.13169, "heading":1.50737, "vx":0.41406, "vy":1.95573, "omega":1.45816, "ax":0.00035, "ay":-0.00009, "alpha":-0.29372, "fx":[1.47652,-1.28913,-1.4648,1.30085], "fy":[1.29355,1.46922,-1.29644,-1.4721]},
+    {"t":3.09945, "x":7.81134, "y":2.17432, "heading":1.53915, "vx":0.41407, "vy":1.95573, "omega":1.45175, "ax":-0.00865, "ay":-0.04143, "alpha":-0.32644, "fx":[1.44519,-1.63377,-1.73074,1.34737], "fy":[0.80545,0.90288,-2.17536,-2.27292]},
+    {"t":3.12125, "x":7.82037, "y":2.21695, "heading":1.5708, "vx":0.41388, "vy":1.95482, "omega":1.44464, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 2f287952..938b7448 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -68,7 +68,8 @@ public enum Action {
     OUTPOST,
     NOTHING,
     OUTPOST_SCORE,
-    CLIMB_ONLY;
+    CLIMB_ONLY,
+    INTAKE_SCORE;
   }
 
   public enum Obstacle {
@@ -126,7 +127,7 @@ public enum Path {
     PLtoIL("PL", "FL", Action.INTAKE),
     // SCORE
     DtoRL("D", "RL", Action.SCORE),
-    OtoRR("O", "RR", Action.SCORE),
+    OtoRR("O", "RR", Action.NOTHING),
     DtoS("D", "S", Action.SCORE),
     OtoS("O", "S", Action.SCORE),
     PMtoM("PM", "M", Action.SCORE),
@@ -140,6 +141,8 @@ public enum Path {
     DtoCL("D", "CL", Action.CLIMB_SCORE),
     RBtoO("RB", "O", Action.OUTPOST_SCORE),
 
+    FRMtoMRScore("FRM", "MR", Action.INTAKE_SCORE),
+
     RUNtoTEST("RUN", "TEST", Action.NOTHING),
 
     BtoD("B", "D", Action.INTAKE);
@@ -218,6 +221,8 @@ public Command runPath(Path path, AutoRoutine routine) {
         return outpostScorePath(path, routine);
       case CLIMB_ONLY:
         return climbNoScorePath(path, routine);
+      case INTAKE_SCORE:
+        return intakeScorePath(path, routine);
       case NOTHING:
         return emptyPath(path, routine);
       default: // this should never happen
@@ -301,6 +306,18 @@ public Command intakePath(Path path, AutoRoutine routine) {
         setAutoIntakeReqFalse());
   }
 
+  public Command intakeScorePath(Path path, AutoRoutine routine) {
+    return Commands.sequence(
+        setAutoScoreReqFalse(),
+        setAutoFlowReqFalse(),
+        setAutoIntakeReqTrue(),
+        path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()),
+        setAutoIntakeReqFalse(),
+        setAutoScoreReqTrue(),
+        swerve.stop().repeatedly().withTimeout(4),
+        setAutoScoreReqFalse());
+  }
+
   public Command flowPath(Path path, AutoRoutine routine) {
     return Commands.sequence(
         setAutoScoreReqTrue(),
@@ -336,8 +353,14 @@ public Command outpostScorePath(Path path, AutoRoutine routine) {
         setAutoScoreReqFalse(),
         setAutoFlowReqFalse(),
         setAutoIntakeReqFalse(),
+        // spin up before we get there
+        // Commands.parallel(
         path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()),
-        setAutoScoreReqTrue(),
+        // Commands.waitUntil(path.getTrajectory(routine).atTimeBeforeEnd(0.2))
+        // .andThen(
+        setAutoScoreReqTrue()
+        // ))
+        ,
         swerve.stop().repeatedly().withTimeout(4),
         setAutoScoreReqFalse()
         // Commands.waitSeconds(1)
@@ -541,6 +564,23 @@ public Command getFillOutpostScoreClimbAuto() {
     return routine.cmd();
   }
 
+  public Command getRightBumpOutpostCenterAuto() {
+    final AutoRoutine routine = factory.newRoutine("Right Bump Outpost Center Auto");
+    lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
+    // Path[] paths = {Path.PRtoIR, Path.FRtoFRM, Path.FRMtoMR, Path.MRtoO, Path.OtoCR};
+    Path[] paths = {Path.RBtoO, Path.OtoRR, Path.RRtoIR, Path.IRtoIRM, Path.FRMtoMRScore};
+    Command autoCommand =
+        paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse());
+
+    for (Path p : paths) {
+      autoCommand = autoCommand.andThen(runPath(p, routine));
+    }
+
+    routine.active().whileTrue(autoCommand);
+
+    return routine.cmd();
+  }
+
   public Command getDepotClimbAuto() {
     final AutoRoutine routine = factory.newRoutine("Depot Climb Auto");
     lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1);
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 88eac0d5..9eb09008 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -760,6 +760,7 @@ private void addAutos() {
     autoChooser.addOption(
         "Left Bump Depot Outpost Climb", autos.getLeftBumpDepotOutpostClimbAuto());
     autoChooser.addOption("Right Bump Outpost Climb", autos.getRightBumpOutpostClimbAuto());
+    autoChooser.addOption("Right Bump Outpost Center", autos.getRightBumpOutpostCenterAuto());
 
     haveAutosGenerated = true;
     System.out.println("Done generating autos");

From 9ae999cc16a9e9d494dd83c983d0d98f50da68ee Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 23:17:57 -0700
Subject: [PATCH 48/54] slow down returning from center

---
 src/main/deploy/choreo/FRMtoMR.traj | 187 +++++++++++++++-------------
 1 file changed, 97 insertions(+), 90 deletions(-)

diff --git a/src/main/deploy/choreo/FRMtoMR.traj b/src/main/deploy/choreo/FRMtoMR.traj
index 0a0701bf..2c29dde0 100644
--- a/src/main/deploy/choreo/FRMtoMR.traj
+++ b/src/main/deploy/choreo/FRMtoMR.traj
@@ -3,28 +3,28 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":7.82036542892456, "y":3.55698299407959, "heading":1.5707963267948966, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":6.916783809661865, "y":0.7018944621086121, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":7.82036542892456, "y":3.55698299407959, "heading":1.5707963267948966, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.916783809661865, "y":0.7018944621086121, "heading":3.141592653589793, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":3.32966947555542, "y":0.688210129737854, "heading":3.141592653589793, "intervals":82, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false},
-    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.3}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true},
     {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":6.0}}, "enabled":false},
     {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}],
   "targetDt":0.05
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"FRM.x", "val":7.82036542892456}, "y":{"exp":"FRM.y", "val":3.55698299407959}, "heading":{"exp":"FRM.heading", "val":1.5707963267948966}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"6.916783809661865 m", "val":6.916783809661865}, "y":{"exp":"0.7018944621086121 m", "val":0.7018944621086121}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"FRM.x", "val":7.82036542892456}, "y":{"exp":"FRM.y", "val":3.55698299407959}, "heading":{"exp":"FRM.heading", "val":1.5707963267948966}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.916783809661865 m", "val":6.916783809661865}, "y":{"exp":"0.7018944621086121 m", "val":0.7018944621086121}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"MR.x", "val":3.32966947555542}, "y":{"exp":"MR.y", "val":0.688210129737854}, "heading":{"exp":"MR.heading", "val":3.141592653589793}, "intervals":82, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
-    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2.3 m / s", "val":2.3}}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.0}}}, "enabled":true},
     {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"6 rad / s", "val":6.0}}}, "enabled":false},
     {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}],
   "targetDt":{
@@ -57,91 +57,98 @@
    "differentialTrackWidth":0.5427218
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.37739,2.93811],
+  "waypoints":[0.0,1.55576,3.35045],
   "samples":[
-    {"t":0.0, "x":7.82037, "y":3.55698, "heading":1.5708, "vx":-0.33365, "vy":-2.27269, "omega":1.31196, "ax":-0.01032, "ay":-0.06999, "alpha":-0.013, "fx":[-0.10932,-0.23196,-0.23196,-0.10932], "fy":[-1.09588,-1.09587,-1.21849,-1.2185]},
-    {"t":0.0313, "x":7.80992, "y":3.4858, "heading":1.61187, "vx":-0.33397, "vy":-2.27488, "omega":1.31155, "ax":0.00001, "ay":-0.00001, "alpha":-0.02344, "fx":[0.10609,-0.11489,-0.10581,0.11517], "fy":[0.11492,0.10584,-0.11514,-0.10606]},
-    {"t":0.06261, "x":7.79946, "y":3.41459, "heading":1.65292, "vx":-0.33397, "vy":-2.27488, "omega":1.31081, "ax":0.00001, "ay":0.0, "alpha":-0.0334, "fx":[0.14436,-0.16973,-0.14388,0.17021], "fy":[0.16994,0.14408,-0.17001,-0.14415]},
-    {"t":0.09391, "x":7.78901, "y":3.34338, "heading":1.69396, "vx":-0.33397, "vy":-2.27488, "omega":1.30977, "ax":0.00001, "ay":0.0, "alpha":-0.04294, "fx":[0.17635,-0.22575,-0.17597,0.22613], "fy":[0.22591,0.17614,-0.22597,-0.17619]},
-    {"t":0.12522, "x":7.77855, "y":3.27216, "heading":1.73496, "vx":-0.33397, "vy":-2.27488, "omega":1.30842, "ax":0.00001, "ay":0.0, "alpha":-0.05215, "fx":[0.2026,-0.2828,-0.20239,0.28301], "fy":[0.28289,0.20248,-0.28292,-0.20251]},
-    {"t":0.15652, "x":7.7681, "y":3.20095, "heading":1.77592, "vx":-0.33397, "vy":-2.27488, "omega":1.30679, "ax":0.0, "ay":0.0, "alpha":-0.06113, "fx":[0.22362,-0.34105,-0.22357,0.34109], "fy":[0.34107,0.22359,-0.34107,-0.2236]},
-    {"t":0.18783, "x":7.75764, "y":3.12974, "heading":1.81683, "vx":-0.33397, "vy":-2.27488, "omega":1.30488, "ax":0.0, "ay":0.0, "alpha":-0.07003, "fx":[0.2399,-0.40095,-0.24002,0.40083], "fy":[0.4009,0.23997,-0.40088,-0.23995]},
-    {"t":0.21913, "x":7.74719, "y":3.05852, "heading":1.85768, "vx":-0.33397, "vy":-2.27488, "omega":1.30269, "ax":-0.00001, "ay":0.0, "alpha":-0.07904, "fx":[0.25197,-0.46327,-0.25226,0.46298], "fy":[0.46315,0.25214,-0.46311,-0.2521]},
-    {"t":0.25044, "x":7.73673, "y":2.98731, "heading":1.89846, "vx":-0.33397, "vy":-2.27488, "omega":1.30021, "ax":-0.00001, "ay":0.0, "alpha":-0.08836, "fx":[0.2603,-0.52905,-0.26074,0.52861], "fy":[0.52886,0.26055,-0.5288,-0.26049]},
-    {"t":0.28174, "x":7.72628, "y":2.91609, "heading":1.93916, "vx":-0.33397, "vy":-2.27488, "omega":1.29745, "ax":-0.00002, "ay":0.0, "alpha":-0.09827, "fx":[0.26526,-0.59969,-0.26584,0.59912], "fy":[0.59945,0.26559,-0.59936,-0.26551]},
-    {"t":0.31304, "x":7.71582, "y":2.84488, "heading":1.97977, "vx":-0.33397, "vy":-2.27488, "omega":1.29437, "ax":-0.00002, "ay":0.0, "alpha":-0.10905, "fx":[0.26708,-0.67692,-0.26777,0.67623], "fy":[0.67663,0.26748,-0.67653,-0.26738]},
-    {"t":0.34435, "x":7.70537, "y":2.77367, "heading":2.02029, "vx":-0.33398, "vy":-2.27488, "omega":1.29096, "ax":-0.00002, "ay":0.0, "alpha":-0.12104, "fx":[0.2658,-0.76282,-0.26658,0.76204], "fy":[0.76248,0.26624,-0.76237,-0.26613]},
-    {"t":0.37565, "x":7.69491, "y":2.70245, "heading":2.06071, "vx":-0.33398, "vy":-2.27488, "omega":1.28717, "ax":-0.00003, "ay":0.0, "alpha":-0.13465, "fx":[0.26117,-0.85979,-0.26202,0.85894], "fy":[0.85943,0.26165,-0.85931,-0.26153]},
-    {"t":0.40696, "x":7.68446, "y":2.63124, "heading":2.101, "vx":-0.33398, "vy":-2.27488, "omega":1.28295, "ax":-0.00003, "ay":0.0, "alpha":-0.15028, "fx":[0.25265,-0.97059,-0.25354,0.96971], "fy":[0.97021,0.25316,-0.97008,-0.25303]},
-    {"t":0.43826, "x":7.674, "y":2.56003, "heading":2.14116, "vx":-0.33398, "vy":-2.27488, "omega":1.27825, "ax":-0.00003, "ay":0.0, "alpha":-0.16843, "fx":[0.23933,-1.09827,-0.24022,1.09738], "fy":[1.09789,0.23984,-1.09776,-0.23971]},
-    {"t":0.46957, "x":7.66355, "y":2.48881, "heading":2.18118, "vx":-0.33398, "vy":-2.27488, "omega":1.27297, "ax":-0.00003, "ay":0.0, "alpha":-0.18962, "fx":[0.21985,-1.24618,-0.22072,1.24531], "fy":[1.24581,0.22035,-1.24568,-0.22022]},
-    {"t":0.50087, "x":7.65309, "y":2.4176, "heading":2.22103, "vx":-0.33398, "vy":-2.27488, "omega":1.26704, "ax":-0.00002, "ay":0.0, "alpha":-0.21443, "fx":[0.19237,-1.41795,-0.19319,1.41713], "fy":[1.4176,0.19284,-1.41748,-0.19272]},
-    {"t":0.53217, "x":7.64264, "y":2.34638, "heading":2.26069, "vx":-0.33398, "vy":-2.27488, "omega":1.26033, "ax":-0.00002, "ay":0.0, "alpha":-0.24349, "fx":[0.15454,-1.61745,-0.15528,1.6167], "fy":[1.61713,0.15496,-1.61702,-0.15486]},
-    {"t":0.56348, "x":7.63218, "y":2.27517, "heading":2.30014, "vx":-0.33398, "vy":-2.27488, "omega":1.2527, "ax":-0.00002, "ay":0.0, "alpha":-0.2775, "fx":[0.10339,-1.84876,-0.10404,1.84811], "fy":[1.84848,0.10376,-1.84839,-0.10367]},
-    {"t":0.59478, "x":7.62173, "y":2.20396, "heading":2.33936, "vx":-0.33398, "vy":-2.27488, "omega":1.24402, "ax":-0.00002, "ay":0.0, "alpha":-0.31719, "fx":[0.03536,-2.11615,-0.03589,2.11561], "fy":[2.11592,0.03566,-2.11584,-0.03559]},
-    {"t":0.62609, "x":7.61127, "y":2.13274, "heading":2.3783, "vx":-0.33398, "vy":-2.27488, "omega":1.23409, "ax":-0.00001, "ay":0.0, "alpha":-0.3634, "fx":[-0.05379,-2.42405,0.05339,2.42365], "fy":[2.42388,-0.05357,-2.42382,0.05362]},
-    {"t":0.65739, "x":7.60082, "y":2.06153, "heading":2.41693, "vx":-0.33398, "vy":-2.27488, "omega":1.22271, "ax":-0.00001, "ay":0.0, "alpha":-0.417, "fx":[-0.16901,-2.77704,0.16875,2.77678], "fy":[2.77692,-0.16886,-2.77689,0.16889]},
-    {"t":0.6887, "x":7.59036, "y":1.99032, "heading":2.45521, "vx":-0.33398, "vy":-2.27488, "omega":1.20966, "ax":0.0, "ay":0.0, "alpha":-0.47895, "fx":[-0.31594,-3.17979,0.31581,3.17967], "fy":[3.17974,-0.31587,-3.17973,0.31588]},
-    {"t":0.72, "x":7.57991, "y":1.9191, "heading":2.49308, "vx":-0.33398, "vy":-2.27488, "omega":1.19466, "ax":0.0, "ay":0.0, "alpha":-0.55031, "fx":[-0.50102,-3.63713,0.50097,3.63709], "fy":[3.63711,-0.50099,-3.63711,0.50099]},
-    {"t":0.75131, "x":7.56945, "y":1.84789, "heading":2.53048, "vx":-0.33398, "vy":-2.27488, "omega":1.17744, "ax":-0.00001, "ay":0.0, "alpha":-0.63221, "fx":[-0.73151,-4.15406,0.73124,4.15379], "fy":[4.15394,-0.73136,-4.15391,0.73139]},
-    {"t":0.78261, "x":7.559, "y":1.77668, "heading":2.56734, "vx":-0.33398, "vy":-2.27488, "omega":1.15765, "ax":-0.00007, "ay":0.00001, "alpha":-0.72587, "fx":[-1.01603,-4.73629,1.0138,4.73407], "fy":[4.73533,-1.01476,-4.73502,1.01508]},
-    {"t":0.81391, "x":7.54854, "y":1.70546, "heading":2.60357, "vx":-0.33399, "vy":-2.27488, "omega":1.13492, "ax":-0.00043, "ay":0.00006, "alpha":-0.83265, "fx":[-1.36744,-5.39315,1.35305,5.3788], "fy":[5.38701,-1.35919,-5.38493,1.3613]},
-    {"t":0.84522, "x":7.53809, "y":1.63425, "heading":2.6391, "vx":-0.334, "vy":-2.27487, "omega":1.10886, "ax":-0.00262, "ay":0.00038, "alpha":-0.95398, "fx":[-1.8201,-6.15485,1.73325,6.06834], "fy":[6.11788,-1.77024,-6.10531,1.78311]},
-    {"t":0.87652, "x":7.52763, "y":1.56303, "heading":2.67381, "vx":-0.33408, "vy":-2.27486, "omega":1.07899, "ax":-0.01555, "ay":0.00229, "alpha":-1.09144, "fx":[-2.53186,-7.17392,2.01634,6.66093], "fy":[6.95464,-2.23573,-6.88033,2.31255]},
-    {"t":0.90783, "x":7.51716, "y":1.49182, "heading":2.70759, "vx":-0.33457, "vy":-2.27479, "omega":1.04483, "ax":-0.09181, "ay":0.01356, "alpha":-1.24636, "fx":[-4.38389,-9.31955,1.33895,6.29267], "fy":[8.02721,-2.63359,-7.58902,3.09233]},
-    {"t":0.93913, "x":7.50664, "y":1.42062, "heading":2.7403, "vx":-0.33744, "vy":-2.27437, "omega":1.00581, "ax":-0.53767, "ay":0.08181, "alpha":-1.40698, "fx":[-12.40638,-17.52422,-5.43406,-0.19367], "fy":[10.05221,-2.15534,-7.42867,4.94247]},
-    {"t":0.97044, "x":7.49582, "y":1.34946, "heading":2.77179, "vx":-0.35427, "vy":-2.27181, "omega":0.96177, "ax":-2.73648, "ay":0.48, "alpha":-1.23281, "fx":[-47.8716,-51.87777,-42.78238,-38.44204], "fy":[16.06731,4.03429,-0.59237,12.23487]},
-    {"t":1.00174, "x":7.48339, "y":1.27858, "heading":2.80189, "vx":-0.43994, "vy":-2.25678, "omega":0.92317, "ax":-5.90617, "ay":1.40742, "alpha":-0.39225, "fx":[-97.445,-98.95969,-97.87889,-96.3138], "fy":[26.59819,20.8656,19.79995,25.81414]},
-    {"t":1.03305, "x":7.46672, "y":1.20862, "heading":2.83079, "vx":-0.62483, "vy":-2.21272, "omega":0.91089, "ax":-6.68115, "ay":2.23875, "alpha":-0.13838, "fx":[-110.16081,-110.92869,-110.77191,-109.98829], "fy":[38.17514,35.87631,35.8308,38.17496]},
-    {"t":1.06435, "x":7.44389, "y":1.14045, "heading":2.85931, "vx":-0.83398, "vy":-2.14264, "omega":0.90656, "ax":-6.66894, "ay":2.98749, "alpha":-0.06999, "fx":[-110.05909,-110.56977,-110.46538,-109.94798], "fy":[49.89799,48.74277,48.88347,50.04957]},
-    {"t":1.09565, "x":7.41451, "y":1.07484, "heading":2.88769, "vx":-1.04274, "vy":-2.04912, "omega":0.90437, "ax":-6.42108, "ay":3.68915, "alpha":-0.04327, "fx":[-106.03079,-106.41165,-106.29611,-105.91193], "fy":[61.24133,60.56988,60.7457,61.42037]},
-    {"t":1.12696, "x":7.37872, "y":1.0125, "heading":2.916, "vx":-1.24375, "vy":-1.93363, "omega":0.90302, "ax":-6.0521, "ay":4.34676, "alpha":-0.03011, "fx":[-99.97792,-100.27747,-100.1469,-99.84559], "fy":[71.99113,71.56923,71.7421,72.16492]},
-    {"t":1.15826, "x":7.33682, "y":0.9541, "heading":2.94427, "vx":-1.43321, "vy":-1.79756, "omega":0.90207, "ax":-5.59676, "ay":4.95684, "alpha":-0.02263, "fx":[-92.48485,-92.72592,-92.58289,-92.34088], "fy":[82.01218,81.73708,81.89486,82.17008]},
-    {"t":1.18957, "x":7.28922, "y":0.90026, "heading":2.97251, "vx":-1.60841, "vy":-1.64239, "omega":0.90137, "ax":-5.06891, "ay":5.51575, "alpha":-0.01793, "fx":[-83.78538,-83.9803,-83.82794,-83.63255], "fy":[91.21576,91.03477,91.17308,91.3539]},
-    {"t":1.22087, "x":7.23638, "y":0.85155, "heading":3.00072, "vx":-1.76709, "vy":-1.46972, "omega":0.9008, "ax":-4.45185, "ay":6.03679, "alpha":-0.01477, "fx":[-73.60641,-73.76232,-73.60253,-73.44645], "fy":[99.80862,99.69239,99.80938,99.92535]},
-    {"t":1.25218, "x":7.17888, "y":0.8085, "heading":3.02892, "vx":-1.90645, "vy":-1.28074, "omega":0.90034, "ax":-3.62623, "ay":6.57355, "alpha":-0.01252, "fx":[-59.97883,-60.09772,-59.92937,-59.81051], "fy":[108.67044,108.60399,108.69645,108.76265]},
-    {"t":1.28348, "x":7.11743, "y":0.77162, "heading":3.05711, "vx":-2.01997, "vy":-1.07496, "omega":0.89995, "ax":-2.8094, "ay":6.96703, "alpha":-0.01085, "fx":[-46.49094,-46.57621,-46.40702,-46.32189], "fy":[115.17252,115.13752,115.20554,115.24034]},
-    {"t":1.31478, "x":7.05281, "y":0.74139, "heading":3.08528, "vx":-2.10792, "vy":-0.85686, "omega":0.89961, "ax":-2.18617, "ay":7.19059, "alpha":-0.00963, "fx":[-36.19655,-36.25622,-36.09329,-36.03379], "fy":[118.86975,118.85115,118.90058,118.91903]},
-    {"t":1.34609, "x":6.98576, "y":0.71809, "heading":3.11344, "vx":-2.17635, "vy":-0.63176, "omega":0.89931, "ax":-1.72135, "ay":7.31841, "alpha":-0.00871, "fx":[-28.5166,-28.55755,-28.40304,-28.36226], "fy":[120.98536,120.97537,121.01166,121.02155]},
-    {"t":1.37739, "x":6.91678, "y":0.70189, "heading":3.14159, "vx":-2.23024, "vy":-0.40267, "omega":0.89904, "ax":-1.61384, "ay":7.33688, "alpha":-0.01413, "fx":[-26.78126,-26.83714,-26.5832,-26.5278], "fy":[121.28256,121.26947,121.32523,121.33809]},
-    {"t":1.41846, "x":6.82382, "y":0.69154, "heading":-3.10467, "vx":-2.29652, "vy":-0.10133, "omega":0.89846, "ax":-0.02253, "ay":4.79886, "alpha":-1.84462, "fx":[-11.93522,-13.26434,13.33539,10.3741], "fy":[84.29097,73.94766,74.40464,84.7232]},
-    {"t":1.45954, "x":6.72948, "y":0.69143, "heading":-3.06777, "vx":-2.29745, "vy":0.09577, "omega":0.82269, "ax":-0.042, "ay":-1.47443, "alpha":-3.4192, "fx":[-19.50097,-15.3222,16.4455,15.60013], "fy":[-10.16735,-40.82846,-38.83745,-7.67656]},
-    {"t":1.50061, "x":6.63509, "y":0.69412, "heading":-3.03398, "vx":-2.29917, "vy":0.03521, "omega":0.68226, "ax":-0.00436, "ay":-0.36019, "alpha":-3.18389, "fx":[-16.82633,-13.25407,16.34398,13.44828], "fy":[7.29475,-22.4243,-19.28275,10.59139]},
-    {"t":1.54168, "x":6.54065, "y":0.69526, "heading":-3.00596, "vx":-2.29935, "vy":0.02042, "omega":0.5515, "ax":-0.00126, "ay":-0.17079, "alpha":-2.77053, "fx":[-14.80562,-11.14953,14.64445,11.22766], "fy":[8.33603,-17.51637,-14.01755,11.90317]},
-    {"t":1.58275, "x":6.44621, "y":0.69596, "heading":-2.98331, "vx":-2.2994, "vy":0.0134, "omega":0.43771, "ax":-0.00055, "ay":-0.11513, "alpha":-2.40037, "fx":[-13.00922,-9.37882,12.93122,9.42028], "fy":[7.48155,-14.85778,-11.30855,11.07051]},
-    {"t":1.62382, "x":6.35177, "y":0.69641, "heading":-2.96533, "vx":-2.29943, "vy":0.00867, "omega":0.33912, "ax":-0.00026, "ay":-0.08617, "alpha":-2.07642, "fx":[-11.3838,-7.91494,11.3422,7.93944], "fy":[6.4942,-12.77864,-9.35595,9.94142]},
-    {"t":1.66489, "x":6.25733, "y":0.69669, "heading":-2.9514, "vx":-2.29944, "vy":0.00513, "omega":0.25384, "ax":-0.00011, "ay":-0.06687, "alpha":-1.79421, "fx":[-9.92386,-6.70435,9.90129,6.71969], "fy":[5.60153,-11.01281,-7.82036,8.80921]},
-    {"t":1.70597, "x":6.16289, "y":0.69685, "heading":-2.94097, "vx":-2.29944, "vy":0.00239, "omega":0.18015, "ax":-0.00003, "ay":-0.0528, "alpha":-1.54897, "fx":[-8.62339,-5.69985,8.61134,5.70992], "fy":[4.82901,-9.48712,-6.57962,7.74591]},
-    {"t":1.74704, "x":6.06845, "y":0.6969, "heading":-2.93358, "vx":-2.29944, "vy":0.00022, "omega":0.11653, "ax":0.00001, "ay":-0.04211, "alpha":-1.33623, "fx":[-7.47299,-4.86268,7.46685,4.8696], "fy":[4.16825,-8.16416,-5.56342,6.77475]},
-    {"t":1.78811, "x":5.97401, "y":0.69687, "heading":-2.92879, "vx":-2.29944, "vy":-0.00151, "omega":0.06165, "ax":0.00003, "ay":-0.03378, "alpha":-1.15194, "fx":[-6.46099,-4.16161,6.45819,4.16656], "fy":[3.60449,-7.017,-4.72333,5.90166]},
-    {"t":1.82918, "x":5.87957, "y":0.69678, "heading":-2.92626, "vx":-2.29944, "vy":-0.0029, "omega":0.01433, "ax":0.00004, "ay":-0.02721, "alpha":-0.99244, "fx":[-5.57469,-3.57156,5.57374,3.57523], "fy":[3.12289,-6.02342,-4.02371,5.12472]},
-    {"t":1.87025, "x":5.78513, "y":0.69664, "heading":-2.92567, "vx":-2.29944, "vy":-0.00402, "omega":-0.02643, "ax":0.00004, "ay":-0.02197, "alpha":-0.8545, "fx":[-4.80121,-3.07248,4.80124,3.07527], "fy":[2.71024,-5.16409,-3.4374,4.4382]},
-    {"t":1.91132, "x":5.69068, "y":0.69646, "heading":-2.92675, "vx":-2.29944, "vy":-0.00492, "omega":-0.06152, "ax":0.00004, "ay":-0.01777, "alpha":-0.73525, "fx":[-4.12806,-2.6483,4.12858,2.65048], "fy":[2.35537,-4.42188,-2.94334,3.83467]},
-    {"t":1.95239, "x":5.59624, "y":0.69624, "heading":-2.92928, "vx":-2.29944, "vy":-0.00565, "omega":-0.09172, "ax":0.00004, "ay":-0.01439, "alpha":-0.63219, "fx":[-3.54348,-2.28609,3.54422,2.28782], "fy":[2.04897,-3.78157,-2.52491,3.30608]},
-    {"t":1.99347, "x":5.5018, "y":0.696, "heading":-2.93305, "vx":-2.29943, "vy":-0.00624, "omega":-0.11768, "ax":0.00003, "ay":-0.01166, "alpha":-0.5431, "fx":[-3.03664,-1.97544,3.03742,1.97683], "fy":[1.78336,-3.22965,-2.16889,2.84438]},
-    {"t":2.03454, "x":5.40736, "y":0.69573, "heading":-2.93788, "vx":-2.29943, "vy":-0.00672, "omega":-0.13999, "ax":0.00003, "ay":-0.00945, "alpha":-0.46607, "fx":[-2.59766,-1.70787,2.59842,1.70899], "fy":[1.5522,-2.75418,-1.86465,2.44189]},
-    {"t":2.07561, "x":5.31292, "y":0.69545, "heading":-2.94363, "vx":-2.29943, "vy":-0.00711, "omega":-0.15913, "ax":0.00002, "ay":-0.00766, "alpha":-0.39942, "fx":[-2.2177,-1.47643,2.21839,1.47734], "fy":[1.35023,-2.34465,-1.60353,2.09144]},
-    {"t":2.11668, "x":5.21848, "y":0.69515, "heading":-2.95017, "vx":-2.29943, "vy":-0.00742, "omega":-0.17554, "ax":0.00002, "ay":-0.00621, "alpha":-0.34168, "fx":[-1.88884,-1.27542,1.88945,1.27616], "fy":[1.17309,-1.99181,-1.37848,1.78648]},
-    {"t":2.15775, "x":5.12404, "y":0.69484, "heading":-2.95738, "vx":-2.29943, "vy":-0.00768, "omega":-0.18957, "ax":0.00002, "ay":-0.00504, "alpha":-0.2916, "fx":[-1.60406,-1.10006,1.60459,1.10067], "fy":[1.01709,-1.68758,-1.18364,1.52107]},
-    {"t":2.19882, "x":5.0296, "y":0.69452, "heading":-2.96516, "vx":-2.29943, "vy":-0.00788, "omega":-0.20155, "ax":0.00001, "ay":-0.00408, "alpha":-0.24805, "fx":[-1.35716,-0.94638,1.35761,0.94687], "fy":[0.87909,-1.42491,-1.01415,1.28987]},
-    {"t":2.2399, "x":4.93515, "y":0.69419, "heading":-2.97344, "vx":-2.29943, "vy":-0.00805, "omega":-0.21174, "ax":0.00001, "ay":-0.00331, "alpha":-0.21006, "fx":[-1.14267,-0.81096,1.14305,0.81136], "fy":[0.7564,-1.19761,-0.86591,1.08811]},
-    {"t":2.28097, "x":4.84071, "y":0.69386, "heading":-2.98214, "vx":-2.29943, "vy":-0.00819, "omega":-0.22036, "ax":0.00001, "ay":-0.00269, "alpha":-0.1768, "fx":[-0.95577,-0.69089,0.95608,0.69122], "fy":[0.64666,-1.00032,-0.73545,0.91154]},
-    {"t":2.32204, "x":4.74627, "y":0.69352, "heading":-2.99119, "vx":-2.29943, "vy":-0.0083, "omega":-0.22762, "ax":0.00001, "ay":-0.00218, "alpha":-0.14752, "fx":[-0.79222,-0.58366,0.79248,0.58393], "fy":[0.54781,-0.82833,-0.61978,0.75636]},
-    {"t":2.36311, "x":4.65183, "y":0.69318, "heading":-3.00054, "vx":-2.29943, "vy":-0.00839, "omega":-0.23368, "ax":0.00001, "ay":-0.00176, "alpha":-0.12156, "fx":[-0.64827,-0.48702,0.64848,0.48724], "fy":[0.45798,-0.67753,-0.51629,0.61922]},
-    {"t":2.40418, "x":4.55739, "y":0.69283, "heading":-3.01013, "vx":-2.29943, "vy":-0.00846, "omega":-0.23868, "ax":0.00001, "ay":-0.00143, "alpha":-0.09833, "fx":[-0.5206,-0.39899,0.52078,0.39917], "fy":[0.37547,-0.5443,-0.42269,0.49708]},
-    {"t":2.44525, "x":4.46295, "y":0.69248, "heading":-3.01994, "vx":-2.29943, "vy":-0.00852, "omega":-0.24271, "ax":0.0, "ay":-0.00116, "alpha":-0.07732, "fx":[-0.40628,-0.31775,0.40642,0.31789], "fy":[0.29872,-0.42545,-0.33692,0.38725]},
-    {"t":2.48632, "x":4.36851, "y":0.69213, "heading":-3.02991, "vx":-2.29943, "vy":-0.00856, "omega":-0.24589, "ax":0.0, "ay":-0.00093, "alpha":-0.05806, "fx":[-0.30266,-0.24161,0.30278,0.24172], "fy":[0.22624,-0.31815,-0.25709,0.28729]},
-    {"t":2.5274, "x":4.27407, "y":0.69178, "heading":-3.04, "vx":-2.29943, "vy":-0.0086, "omega":-0.24827, "ax":0.0, "ay":-0.00075, "alpha":-0.0401, "fx":[-0.20736,-0.16899,0.20745,0.16908], "fy":[0.1566,-0.21984,-0.18147,0.19497]},
-    {"t":2.56847, "x":4.17963, "y":0.69143, "heading":-3.0502, "vx":-2.29943, "vy":-0.00863, "omega":-0.24992, "ax":0.0, "ay":-0.0006, "alpha":-0.02305, "fx":[-0.11819,-0.09834,0.11826,0.09841], "fy":[0.08839,-0.12821,-0.10836,0.10824]},
-    {"t":2.60954, "x":4.08519, "y":0.69107, "heading":-3.06047, "vx":-2.29943, "vy":-0.00866, "omega":-0.25087, "ax":0.0, "ay":-0.00048, "alpha":-0.00652, "fx":[-0.03313,-0.02814,0.03319,0.0282], "fy":[0.0202,-0.04114,-0.03615,0.02518]},
-    {"t":2.65061, "x":3.99074, "y":0.69072, "heading":-3.07077, "vx":-2.29943, "vy":-0.00868, "omega":-0.25114, "ax":0.0, "ay":-0.00038, "alpha":0.00986, "fx":[0.04973,0.04314,-0.04968,-0.04309], "fy":[-0.04944,0.04338,0.0368,-0.05602]},
-    {"t":2.69168, "x":3.8963, "y":0.69036, "heading":-3.08108, "vx":-2.29943, "vy":-0.00869, "omega":-0.25073, "ax":0.0, "ay":-0.0003, "alpha":0.02647, "fx":[0.13222,0.11711,-0.13218,-0.11708], "fy":[-0.12203,0.12726,0.11216,-0.13714]},
-    {"t":2.73275, "x":3.80186, "y":0.69, "heading":-3.09138, "vx":-2.29943, "vy":-0.00871, "omega":-0.24964, "ax":0.0, "ay":-0.00023, "alpha":0.04368, "fx":[0.21614,0.19546,-0.21611,-0.19543], "fy":[-0.19922,0.21235,0.19167,-0.2199]},
-    {"t":2.77383, "x":3.70742, "y":0.68964, "heading":-3.10164, "vx":-2.29943, "vy":-0.00872, "omega":-0.24785, "ax":0.0, "ay":-0.00017, "alpha":0.06187, "fx":[0.3033,0.27998,-0.30328,-0.27996], "fy":[-0.28275,0.30051,0.27719,-0.30606]},
-    {"t":2.8149, "x":3.61298, "y":0.68929, "heading":-3.11182, "vx":-2.29943, "vy":-0.00872, "omega":-0.24531, "ax":0.0, "ay":-0.00011, "alpha":0.08145, "fx":[0.39554,0.37266,-0.39553,-0.37265], "fy":[-0.37455,0.39363,0.37075,-0.39744]},
-    {"t":2.85597, "x":3.51854, "y":0.68893, "heading":-3.12189, "vx":-2.29943, "vy":-0.00873, "omega":-0.24196, "ax":0.0, "ay":-0.00007, "alpha":0.10288, "fx":[0.49481,0.47569,-0.49479,-0.47567], "fy":[-0.47679,0.49369,0.47457,-0.49591]},
-    {"t":2.89704, "x":3.4241, "y":0.68857, "heading":-3.13183, "vx":-2.29943, "vy":-0.00873, "omega":-0.23774, "ax":0.01362, "ay":0.00003, "alpha":0.12662, "fx":[0.82835,0.81668,-0.37794,-0.36627], "fy":[-0.59098,0.60363,0.59198,-0.60267]},
-    {"t":2.93811, "x":3.32967, "y":0.68821, "heading":3.14159, "vx":-2.29887, "vy":-0.00873, "omega":-0.23254, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":7.82037, "y":3.55698, "heading":1.5708, "vx":-0.39133, "vy":-1.95887, "omega":1.16616, "ax":-0.01076, "ay":-0.05382, "alpha":-0.00645, "fx":[-0.14751,-0.20837,-0.20837,-0.14751], "fy":[-0.85946,-0.85946,-0.92031,-0.92032]},
+    {"t":0.0331, "x":7.80741, "y":3.49211, "heading":1.6094, "vx":-0.39169, "vy":-1.96065, "omega":1.16595, "ax":0.0, "ay":0.0, "alpha":-0.01209, "fx":[0.05481,-0.05922,-0.05481,0.05922], "fy":[0.05917,0.05477,-0.05926,-0.05486]},
+    {"t":0.0662, "x":7.79444, "y":3.42721, "heading":1.64799, "vx":-0.39169, "vy":-1.96065, "omega":1.16555, "ax":0.0, "ay":0.0, "alpha":-0.01771, "fx":[0.07688,-0.08973,-0.07684,0.08977], "fy":[0.08975,0.07686,-0.08976,-0.07687]},
+    {"t":0.0993, "x":7.78148, "y":3.36231, "heading":1.68657, "vx":-0.39169, "vy":-1.96065, "omega":1.16496, "ax":0.0, "ay":0.0, "alpha":-0.02336, "fx":[0.09674,-0.12217,-0.09671,0.12219], "fy":[0.12218,0.09672,-0.12218,-0.09673]},
+    {"t":0.13241, "x":7.76851, "y":3.29741, "heading":1.72513, "vx":-0.39169, "vy":-1.96065, "omega":1.16419, "ax":0.0, "ay":0.0, "alpha":-0.02909, "fx":[0.11452,-0.1567,-0.11451,0.15671], "fy":[0.15671,0.11451,-0.15671,-0.11451]},
+    {"t":0.16551, "x":7.75554, "y":3.23251, "heading":1.76367, "vx":-0.39169, "vy":-1.96065, "omega":1.16322, "ax":0.0, "ay":0.0, "alpha":-0.03498, "fx":[0.13033,-0.1936,-0.13034,0.19359], "fy":[0.1936,0.13033,-0.1936,-0.13033]},
+    {"t":0.19861, "x":7.74258, "y":3.16761, "heading":1.80218, "vx":-0.39169, "vy":-1.96065, "omega":1.16206, "ax":0.0, "ay":0.0, "alpha":-0.0411, "fx":[0.14425,-0.2332,-0.14428,0.23317], "fy":[0.23319,0.14426,-0.23318,-0.14426]},
+    {"t":0.23171, "x":7.72961, "y":3.10271, "heading":1.84064, "vx":-0.39169, "vy":-1.96065, "omega":1.1607, "ax":0.0, "ay":0.0, "alpha":-0.04753, "fx":[0.15631,-0.27591,-0.15636,0.27586], "fy":[0.27589,0.15634,-0.27588,-0.15633]},
+    {"t":0.26481, "x":7.71665, "y":3.03781, "heading":1.87906, "vx":-0.39169, "vy":-1.96065, "omega":1.15913, "ax":0.0, "ay":0.0, "alpha":-0.05436, "fx":[0.16652,-0.32221,-0.16659,0.32214], "fy":[0.32218,0.16656,-0.32217,-0.16655]},
+    {"t":0.29791, "x":7.70368, "y":2.97291, "heading":1.91743, "vx":-0.39169, "vy":-1.96065, "omega":1.15733, "ax":0.0, "ay":0.0, "alpha":-0.0617, "fx":[0.17483,-0.37267,-0.1749,0.37259], "fy":[0.37264,0.17487,-0.37262,-0.17486]},
+    {"t":0.33101, "x":7.69072, "y":2.90801, "heading":1.95574, "vx":-0.39169, "vy":-1.96065, "omega":1.15529, "ax":0.0, "ay":0.0, "alpha":-0.06965, "fx":[0.18109,-0.42794,-0.18118,0.42785], "fy":[0.4279,0.18115,-0.42788,-0.18113]},
+    {"t":0.36411, "x":7.67775, "y":2.84311, "heading":1.99398, "vx":-0.39169, "vy":-1.96065, "omega":1.15298, "ax":0.0, "ay":0.0, "alpha":-0.07833, "fx":[0.18513,-0.48874,-0.18523,0.48864], "fy":[0.4887,0.18519,-0.48868,-0.18517]},
+    {"t":0.39722, "x":7.66479, "y":2.77821, "heading":2.03215, "vx":-0.39169, "vy":-1.96065, "omega":1.15039, "ax":0.0, "ay":0.0, "alpha":-0.08789, "fx":[0.18665,-0.5559,-0.18675,0.55579], "fy":[0.55585,0.18671,-0.55583,-0.18669]},
+    {"t":0.43032, "x":7.65182, "y":2.71331, "heading":2.07023, "vx":-0.39169, "vy":-1.96065, "omega":1.14748, "ax":0.0, "ay":0.0, "alpha":-0.09846, "fx":[0.18525,-0.63029,-0.18536,0.63018], "fy":[0.63024,0.18532,-0.63022,-0.18529]},
+    {"t":0.46342, "x":7.63886, "y":2.64841, "heading":2.10821, "vx":-0.39169, "vy":-1.96065, "omega":1.14422, "ax":0.0, "ay":0.0, "alpha":-0.11022, "fx":[0.18043,-0.71289,-0.18054,0.71278], "fy":[0.71285,0.1805,-0.71282,-0.18048]},
+    {"t":0.49652, "x":7.62589, "y":2.58351, "heading":2.14609, "vx":-0.39169, "vy":-1.96065, "omega":1.14057, "ax":0.0, "ay":0.0, "alpha":-0.12333, "fx":[0.17155,-0.80475,-0.17166,0.80464], "fy":[0.8047,0.17162,-0.80468,-0.1716]},
+    {"t":0.52962, "x":7.61293, "y":2.51861, "heading":2.18384, "vx":-0.39169, "vy":-1.96065, "omega":1.13649, "ax":0.0, "ay":0.0, "alpha":-0.13798, "fx":[0.15783,-0.90697,-0.15793,0.90687], "fy":[0.90693,0.15789,-0.90691,-0.15787]},
+    {"t":0.56272, "x":7.59996, "y":2.45371, "heading":2.22146, "vx":-0.39169, "vy":-1.96065, "omega":1.13192, "ax":0.0, "ay":0.0, "alpha":-0.15439, "fx":[0.13832,-1.02076,-0.13841,1.02067], "fy":[1.02072,0.13837,-1.0207,-0.13836]},
+    {"t":0.59582, "x":7.587, "y":2.38881, "heading":2.25893, "vx":-0.39169, "vy":-1.96065, "omega":1.12681, "ax":0.0, "ay":0.0, "alpha":-0.17279, "fx":[0.11191,-1.14735,-0.11199,1.14727], "fy":[1.14732,0.11196,-1.1473,-0.11194]},
+    {"t":0.62893, "x":7.57403, "y":2.32391, "heading":2.29623, "vx":-0.39169, "vy":-1.96065, "omega":1.12109, "ax":0.0, "ay":0.0, "alpha":-0.19341, "fx":[0.0773,-1.28806,-0.07737,1.28799], "fy":[1.28803,0.07734,-1.28802,-0.07733]},
+    {"t":0.66203, "x":7.56106, "y":2.25901, "heading":2.33334, "vx":-0.39169, "vy":-1.96065, "omega":1.11469, "ax":0.0, "ay":0.0, "alpha":-0.21653, "fx":[0.03299,-1.44426,-0.03305,1.4442], "fy":[1.44424,0.03302,-1.44423,-0.03301]},
+    {"t":0.69513, "x":7.5481, "y":2.19411, "heading":2.37023, "vx":-0.39169, "vy":-1.96065, "omega":1.10752, "ax":0.0, "ay":0.0, "alpha":-0.24245, "fx":[-0.02273,-1.61737,0.02269,1.61733], "fy":[1.61736,-0.0227,-1.61735,0.02271]},
+    {"t":0.72823, "x":7.53513, "y":2.12921, "heading":2.40689, "vx":-0.39169, "vy":-1.96065, "omega":1.0995, "ax":0.0, "ay":0.0, "alpha":-0.27148, "fx":[-0.0918,-1.80888,0.09178,1.80886], "fy":[1.80887,-0.09179,-1.80887,0.09179]},
+    {"t":0.76133, "x":7.52217, "y":2.06431, "heading":2.44329, "vx":-0.39169, "vy":-1.96065, "omega":1.09051, "ax":0.0, "ay":0.0, "alpha":-0.30398, "fx":[-0.17641,-2.02032,0.1764,2.02031], "fy":[2.02032,-0.17641,-2.02032,0.17641]},
+    {"t":0.79443, "x":7.5092, "y":1.99941, "heading":2.47939, "vx":-0.39169, "vy":-1.96065, "omega":1.08045, "ax":0.0, "ay":0.0, "alpha":-0.34032, "fx":[-0.279,-2.25328,0.279,2.25329], "fy":[2.25329,-0.279,-2.25329,0.279]},
+    {"t":0.82753, "x":7.49624, "y":1.93451, "heading":2.51515, "vx":-0.39169, "vy":-1.96065, "omega":1.06919, "ax":0.0, "ay":0.0, "alpha":-0.38094, "fx":[-0.40228,-2.50943,0.4023,2.50945], "fy":[2.50943,-0.40229,-2.50944,0.40228]},
+    {"t":0.86063, "x":7.48327, "y":1.86961, "heading":2.55054, "vx":-0.39169, "vy":-1.96065, "omega":1.05658, "ax":0.0, "ay":0.0, "alpha":-0.42629, "fx":[-0.54924,-2.79047,0.54928,2.7905], "fy":[2.79048,-0.54926,-2.79049,0.54926]},
+    {"t":0.89374, "x":7.47031, "y":1.80471, "heading":2.58552, "vx":-0.39169, "vy":-1.96065, "omega":1.04247, "ax":0.0, "ay":0.0, "alpha":-0.47688, "fx":[-0.7232,-3.09822,0.72324,3.09826], "fy":[3.09823,-0.72322,-3.09824,0.72321]},
+    {"t":0.92684, "x":7.45734, "y":1.73981, "heading":2.62002, "vx":-0.39169, "vy":-1.96065, "omega":1.02668, "ax":0.0, "ay":0.0, "alpha":-0.53326, "fx":[-0.92774,-3.43456,0.92779,3.4346], "fy":[3.43457,-0.92778,-3.43459,0.92776]},
+    {"t":0.95994, "x":7.44438, "y":1.6749, "heading":2.65401, "vx":-0.39169, "vy":-1.96065, "omega":1.00903, "ax":0.0, "ay":0.0, "alpha":-0.59604, "fx":[-1.16682,-3.80148,1.16687,3.80153], "fy":[3.8015,-1.16685,-3.80151,1.16684]},
+    {"t":0.99304, "x":7.43141, "y":1.61, "heading":2.68741, "vx":-0.39169, "vy":-1.96065, "omega":0.9893, "ax":0.0, "ay":0.0, "alpha":-0.6659, "fx":[-1.4447,-4.20115,1.4447,4.20115], "fy":[4.20115,-1.4447,-4.20115,1.4447]},
+    {"t":1.02614, "x":7.41845, "y":1.5451, "heading":2.72016, "vx":-0.39169, "vy":-1.96065, "omega":0.96726, "ax":-0.00001, "ay":0.0, "alpha":-0.74356, "fx":[-1.76613,-4.636,1.76571,4.63558], "fy":[4.63583,-1.76588,-4.63575,1.76596]},
+    {"t":1.05924, "x":7.40548, "y":1.4802, "heading":2.75217, "vx":-0.39169, "vy":-1.96065, "omega":0.94264, "ax":-0.00012, "ay":0.00002, "alpha":-0.82983, "fx":[-2.13735,-5.10983,2.13347,5.10596], "fy":[5.10828,-2.13502,-5.10751,2.1358]},
+    {"t":1.09234, "x":7.39251, "y":1.4153, "heading":2.78338, "vx":-0.39169, "vy":-1.96065, "omega":0.91518, "ax":-0.00098, "ay":0.0002, "alpha":-0.92558, "fx":[-2.57465,-5.6364,2.54212,5.60395], "fy":[5.62339,-2.5551,-5.61697,2.56166]},
+    {"t":1.12545, "x":7.37955, "y":1.3504, "heading":2.81367, "vx":-0.39172, "vy":-1.96065, "omega":0.88454, "ax":-0.00813, "ay":0.00163, "alpha":-1.03175, "fx":[-3.17497,-6.30991,2.90561,6.04129], "fy":[6.20213,-3.01306,-6.14912,3.06757]},
+    {"t":1.15855, "x":7.36658, "y":1.2855, "heading":2.84295, "vx":-0.39199, "vy":-1.96059, "omega":0.85039, "ax":-0.06717, "ay":0.01347, "alpha":-1.14919, "fx":[-4.69797,-7.88492,2.47358,5.6672], "fy":[6.99562,-3.36015,-6.55815,3.81344]},
+    {"t":1.19165, "x":7.35357, "y":1.22061, "heading":2.8711, "vx":-0.39422, "vy":-1.96015, "omega":0.81235, "ax":-0.55026, "ay":0.11334, "alpha":-1.26547, "fx":[-13.23183,-16.40325,-4.99186,-1.76402], "fy":[9.20985,-2.28085,-5.54311,6.10966]},
+    {"t":1.22475, "x":7.34022, "y":1.15579, "heading":2.89799, "vx":-0.41243, "vy":-1.95639, "omega":0.77046, "ax":-3.4952, "ay":0.84637, "alpha":-0.89718, "fx":[-59.75307,-62.15758,-55.90284,-53.33725], "fy":[19.6765,9.90858,7.9833,18.40513]},
+    {"t":1.25785, "x":7.32465, "y":1.0915, "heading":2.92349, "vx":-0.52813, "vy":-1.92838, "omega":0.74076, "ax":-6.35042, "ay":2.12464, "alpha":-0.18592, "fx":[-104.75039,-105.68969,-105.25345,-104.28392], "fy":[36.49925,33.56098,33.72056,36.72981]},
+    {"t":1.29095, "x":7.30369, "y":1.02883, "heading":2.94801, "vx":-0.73833, "vy":-1.85805, "omega":0.7346, "ax":-6.57298, "ay":3.08272, "alpha":-0.06441, "fx":[-108.51826,-108.98846,-108.83291,-108.35642], "fy":[51.36331,50.33326,50.56888,51.60672]},
+    {"t":1.32405, "x":7.27565, "y":0.96901, "heading":2.97233, "vx":-0.95591, "vy":-1.75601, "omega":0.73247, "ax":-6.27233, "ay":3.93355, "alpha":-0.03297, "fx":[-103.62121,-103.9172,-103.78612,-103.48813], "fy":[65.17936,64.69884,64.89038,65.37217]},
+    {"t":1.35716, "x":7.24057, "y":0.91304, "heading":2.99657, "vx":-1.16353, "vy":-1.6258, "omega":0.73138, "ax":-5.78316, "ay":4.70967, "alpha":-0.02058, "fx":[-95.57578,-95.78229,-95.65563,-95.44837], "fy":[77.92017,77.66307,77.81379,78.07101]},
+    {"t":1.39026, "x":7.19889, "y":0.86181, "heading":3.02078, "vx":-1.35496, "vy":-1.46991, "omega":0.7307, "ax":-5.17314, "ay":5.40864, "alpha":-0.01442, "fx":[-85.5172,-85.66738,-85.54268,-85.39222], "fy":[89.43738,89.292,89.40959,89.55484]},
+    {"t":1.42336, "x":7.1512, "y":0.81611, "heading":3.04497, "vx":-1.5262, "vy":-1.29087, "omega":0.73022, "ax":-4.46301, "ay":6.02586, "alpha":-0.0109, "fx":[-73.79528,-73.90507,-73.78256,-73.6727], "fy":[99.62439,99.5421,99.63216,99.71429]},
+    {"t":1.45646, "x":7.09824, "y":0.77669, "heading":3.06914, "vx":-1.67393, "vy":-1.09141, "omega":0.72986, "ax":-3.60968, "ay":6.58258, "alpha":-0.00868, "fx":[-59.70189,-59.77943,-59.65904,-59.58153], "fy":[108.82138,108.77827,108.844,108.88699]},
+    {"t":1.48956, "x":7.04085, "y":0.74416, "heading":3.0933, "vx":-1.79342, "vy":-0.87352, "omega":0.72957, "ax":-2.62569, "ay":7.03928, "alpha":-0.00718, "fx":[-43.44495,-43.49481,-43.37834,-43.32855], "fy":[116.37143,116.35245,116.39577,116.41467]},
+    {"t":1.52266, "x":6.98005, "y":0.71911, "heading":3.11745, "vx":-1.88033, "vy":-0.64051, "omega":0.72934, "ax":-1.8637, "ay":7.28221, "alpha":-0.00614, "fx":[-30.85212,-30.88247,-30.77465,-30.74438], "fy":[120.39031,120.38228,120.40982,120.4178]},
+    {"t":1.55576, "x":6.91678, "y":0.70189, "heading":3.14159, "vx":-1.94202, "vy":-0.39946, "omega":0.72913, "ax":-1.29987, "ay":7.39727, "alpha":-0.00987, "fx":[-21.56559,-21.59729,-21.41693,-21.38548], "fy":[122.28956,122.28342,122.31504,122.32109]},
+    {"t":1.59849, "x":6.83261, "y":0.69158, "heading":-3.11044, "vx":-1.99757, "vy":-0.08336, "omega":0.72871, "ax":-0.00739, "ay":3.85284, "alpha":-1.6875, "fx":[-9.81688,-10.51859,10.87543,8.97127], "fy":[69.38904,57.62944,58.03471,69.7494]},
+    {"t":1.64122, "x":6.74725, "y":0.69153, "heading":-3.0793, "vx":-1.99788, "vy":0.08127, "omega":0.6566, "ax":-0.03317, "ay":-1.18511, "alpha":-2.39464, "fx":[-13.16498,-11.04551,11.41579,10.60129], "fy":[-9.3185,-31.21489,-29.95894,-7.88377]},
+    {"t":1.68396, "x":6.66185, "y":0.69392, "heading":-3.05124, "vx":-1.9993, "vy":0.03063, "omega":0.55428, "ax":-0.00331, "ay":-0.26533, "alpha":-2.20751, "fx":[-11.44057,-9.438,11.2105,9.44904], "fy":[5.02172,-15.67185,-13.81875,6.92175]},
+    {"t":1.72669, "x":6.57641, "y":0.69499, "heading":-3.02756, "vx":-1.99944, "vy":0.01929, "omega":0.45995, "ax":-0.00112, "ay":-0.13685, "alpha":-1.94511, "fx":[-10.20646,-8.06966,10.12165,8.08041], "fy":[5.80136,-12.41411,-10.33804,7.9004]},
+    {"t":1.76942, "x":6.49098, "y":0.69569, "heading":-3.0079, "vx":-1.99949, "vy":0.01344, "omega":0.37683, "ax":-0.00055, "ay":-0.09633, "alpha":-1.71034, "fx":[-9.09551,-6.91843,9.05162,6.92605], "fy":[5.32364,-10.6597,-8.5163,7.48145]},
+    {"t":1.81215, "x":6.40554, "y":0.69618, "heading":-2.9918, "vx":-1.99951, "vy":0.00933, "omega":0.30375, "ax":-0.00029, "ay":-0.07361, "alpha":-1.50285, "fx":[-8.08139,-5.95008,8.0569,5.9557], "fy":[4.73242,-9.28221,-7.17107,6.85299]},
+    {"t":1.85488, "x":6.32009, "y":0.69651, "heading":-2.97882, "vx":-1.99952, "vy":0.00618, "omega":0.23953, "ax":-0.00014, "ay":-0.05812, "alpha":-1.31987, "fx":[-7.16064,-5.13334,7.1468,5.13769], "fy":[4.17244,-8.11223,-6.09736,6.1935]},
+    {"t":1.89761, "x":6.23465, "y":0.69672, "heading":-2.96858, "vx":-1.99953, "vy":0.0037, "omega":0.18313, "ax":-0.00006, "ay":-0.04673, "alpha":-1.15871, "fx":[-6.32974,-4.44208,6.32206,4.44559], "fy":[3.66995,-7.09693,-5.21703,5.5539]},
+    {"t":1.94034, "x":6.14921, "y":0.69684, "heading":-2.96076, "vx":-1.99953, "vy":0.0017, "omega":0.13362, "ax":-0.00002, "ay":-0.03798, "alpha":-1.01687, "fx":[-5.58384,-3.85479,5.5798,3.85772], "fy":[3.2274,-6.20888,-4.48471,4.9542]},
+    {"t":1.98307, "x":6.06377, "y":0.69687, "heading":-2.95505, "vx":-1.99953, "vy":0.00008, "omega":0.09017, "ax":0.00001, "ay":-0.0311, "alpha":-0.89212, "fx":[-4.91714,-3.35385,4.91526,3.35634], "fy":[2.84039,-5.42977,-3.86958,4.40229]},
+    {"t":2.0258, "x":5.97833, "y":0.69685, "heading":-2.9512, "vx":-1.99953, "vy":-0.00125, "omega":0.05205, "ax":0.00002, "ay":-0.02558, "alpha":-0.78243, "fx":[-4.32333,-2.92486,4.32271,2.927], "fy":[2.50263,-4.74561,-3.34909,3.90024]},
+    {"t":2.06853, "x":5.89289, "y":0.69677, "heading":-2.94897, "vx":-1.99953, "vy":-0.00234, "omega":0.01862, "ax":0.00003, "ay":-0.02111, "alpha":-0.68602, "fx":[-3.79597,-2.55602,3.79607,2.55787], "fy":[2.2077,-4.14481,-2.90611,3.44711]},
+    {"t":2.11126, "x":5.80745, "y":0.69665, "heading":-2.94818, "vx":-1.99953, "vy":-0.00325, "omega":-0.0107, "ax":0.00003, "ay":-0.01746, "alpha":-0.60129, "fx":[-3.32872,-2.2377,3.32921,2.2393], "fy":[1.94973,-3.61745,-2.52722,3.04041]},
+    {"t":2.15399, "x":5.722, "y":0.6965, "heading":-2.94863, "vx":-1.99953, "vy":-0.00399, "omega":-0.03639, "ax":0.00003, "ay":-0.01446, "alpha":-0.52684, "fx":[-2.91551,-1.96195,2.91619,1.96333], "fy":[1.72352,-3.1548,-2.20174,2.67686]},
+    {"t":2.19672, "x":5.63656, "y":0.69631, "heading":-2.95019, "vx":-1.99953, "vy":-0.00461, "omega":-0.0589, "ax":0.00003, "ay":-0.01199, "alpha":-0.4614, "fx":[-2.55065,-1.72225,2.55139,1.72344], "fy":[1.52463,-2.74912,-1.92105,2.35289]},
+    {"t":2.23945, "x":5.55112, "y":0.69611, "heading":-2.95271, "vx":-1.99953, "vy":-0.00512, "omega":-0.07862, "ax":0.00003, "ay":-0.00994, "alpha":-0.40389, "fx":[-2.22885,-1.51319,2.22959,1.51421], "fy":[1.34928,-2.39357,-1.67811,2.06485]},
+    {"t":2.28218, "x":5.46568, "y":0.69588, "heading":-2.95607, "vx":-1.99953, "vy":-0.00555, "omega":-0.09588, "ax":0.00002, "ay":-0.00825, "alpha":-0.35331, "fx":[-1.94528,-1.33027,1.94597,1.33114], "fy":[1.19425,-2.08203,-1.46715,1.80921]},
+    {"t":2.32492, "x":5.38024, "y":0.69563, "heading":-2.96016, "vx":-1.99952, "vy":-0.0059, "omega":-0.11098, "ax":0.00002, "ay":-0.00685, "alpha":-0.30882, "fx":[-1.69552,-1.16972,1.69615,1.17046], "fy":[1.05681,-1.80908,-1.28336,1.58258]},
+    {"t":2.36765, "x":5.2948, "y":0.69537, "heading":-2.9649, "vx":-1.99952, "vy":-0.00619, "omega":-0.12417, "ax":0.00002, "ay":-0.00569, "alpha":-0.26965, "fx":[-1.4756,-1.02837,1.47616,1.029], "fy":[0.93463,-1.56992,-1.12274,1.38184]},
+    {"t":2.41038, "x":5.20936, "y":0.69511, "heading":-2.97021, "vx":-1.99952, "vy":-0.00643, "omega":-0.13569, "ax":0.00002, "ay":-0.00472, "alpha":-0.23513, "fx":[-1.28193,-0.90355,1.28242,0.90409], "fy":[0.82572,-1.36027,-0.98193,1.20408]},
+    {"t":2.45311, "x":5.12392, "y":0.69483, "heading":-2.97601, "vx":-1.99952, "vy":-0.00664, "omega":-0.14574, "ax":0.00001, "ay":-0.00392, "alpha":-0.20467, "fx":[-1.11129,-0.79297,1.11171,0.79343], "fy":[0.72834,-1.17636,-0.85806,1.04664]},
+    {"t":2.49584, "x":5.03848, "y":0.69454, "heading":-2.98224, "vx":-1.99952, "vy":-0.0068, "omega":-0.15449, "ax":0.00001, "ay":-0.00326, "alpha":-0.17775, "fx":[-0.96081,-0.69467,0.96117,0.69505], "fy":[0.64099,-1.01486,-0.74873,0.90712]},
+    {"t":2.53857, "x":4.95304, "y":0.69425, "heading":-2.98884, "vx":-1.99952, "vy":-0.00694, "omega":-0.16208, "ax":0.00001, "ay":-0.00271, "alpha":-0.1539, "fx":[-0.82791,-0.60695,0.82821,0.60727], "fy":[0.56237,-0.87279,-0.65185,0.78333]},
+    {"t":2.5813, "x":4.86759, "y":0.69395, "heading":-2.99576, "vx":-1.99952, "vy":-0.00706, "omega":-0.16866, "ax":0.00001, "ay":-0.00225, "alpha":-0.13271, "fx":[-0.71029,-0.52833,0.71055,0.5286], "fy":[0.49131,-0.74756,-0.56561,0.67327]},
+    {"t":2.62403, "x":4.78215, "y":0.69364, "heading":-3.00297, "vx":-1.99952, "vy":-0.00715, "omega":-0.17433, "ax":0.00001, "ay":-0.00187, "alpha":-0.11382, "fx":[-0.6059,-0.4575,0.60612,0.45773], "fy":[0.42678,-0.63685,-0.48846,0.57517]},
+    {"t":2.66676, "x":4.69671, "y":0.69333, "heading":-3.01042, "vx":-1.99952, "vy":-0.00723, "omega":-0.17919, "ax":0.00001, "ay":-0.00155, "alpha":-0.0969, "fx":[-0.51291,-0.39332,0.51309,0.39351], "fy":[0.36782,-0.5386,-0.41901,0.48741]},
+    {"t":2.70949, "x":4.61127, "y":0.69302, "heading":-3.01808, "vx":-1.99952, "vy":-0.0073, "omega":-0.18333, "ax":0.0, "ay":-0.00128, "alpha":-0.08166, "fx":[-0.42967,-0.33475,0.42983,0.33491], "fy":[0.3136,-0.45099,-0.35606,0.40852]},
+    {"t":2.75222, "x":4.52583, "y":0.69271, "heading":-3.02591, "vx":-1.99952, "vy":-0.00736, "omega":-0.18682, "ax":0.0, "ay":-0.00106, "alpha":-0.06783, "fx":[-0.35471,-0.28085,0.35484,0.28098], "fy":[0.26331,-0.37238,-0.29852,0.33717]},
+    {"t":2.79495, "x":4.44039, "y":0.6924, "heading":-3.03389, "vx":-1.99952, "vy":-0.0074, "omega":-0.18972, "ax":0.0, "ay":-0.00088, "alpha":-0.05517, "fx":[-0.2867,-0.23074,0.28681,0.23085], "fy":[0.21622,-0.30134,-0.24538,0.27217]},
+    {"t":2.83768, "x":4.35495, "y":0.69208, "heading":-3.042, "vx":-1.99952, "vy":-0.00744, "omega":-0.19208, "ax":0.0, "ay":-0.00073, "alpha":-0.04348, "fx":[-0.22443,-0.18364,0.22452,0.18373], "fy":[0.17163,-0.23653,-0.19575,0.21241]},
+    {"t":2.88041, "x":4.26951, "y":0.69176, "heading":-3.05021, "vx":-1.99952, "vy":-0.00747, "omega":-0.19394, "ax":0.0, "ay":-0.0006, "alpha":-0.03253, "fx":[-0.16679,-0.13878,0.16686,0.13885], "fy":[0.12886,-0.17678,-0.14877,0.15687]},
+    {"t":2.92315, "x":4.18407, "y":0.69144, "heading":-3.0585, "vx":-1.99952, "vy":-0.0075, "omega":-0.19533, "ax":0.0, "ay":-0.0005, "alpha":-0.02215, "fx":[-0.11276,-0.09542,0.11282,0.09548], "fy":[0.08725,-0.12099,-0.10364,0.1046]},
+    {"t":2.96588, "x":4.09863, "y":0.69112, "heading":-3.06684, "vx":-1.99952, "vy":-0.00752, "omega":-0.19627, "ax":0.0, "ay":-0.00041, "alpha":-0.01215, "fx":[-0.06141,-0.05285,0.06146,0.0529], "fy":[0.04616,-0.06815,-0.05959,0.05472]},
+    {"t":3.00861, "x":4.01319, "y":0.6908, "heading":-3.07523, "vx":-1.99952, "vy":-0.00753, "omega":-0.19679, "ax":0.0, "ay":-0.00033, "alpha":-0.00236, "fx":[-0.01183,-0.01036,0.01188,0.0104], "fy":[0.00491,-0.01732,-0.01584,0.00639]},
+    {"t":3.05134, "x":3.92775, "y":0.69048, "heading":-3.08364, "vx":-1.99952, "vy":-0.00755, "omega":-0.19689, "ax":0.0, "ay":-0.00027, "alpha":0.00739, "fx":[0.03682,0.03278,-0.03678,-0.03275], "fy":[-0.03717,0.03239,0.02836,-0.04121]},
+    {"t":3.09407, "x":3.8423, "y":0.69015, "heading":-3.09205, "vx":-1.99952, "vy":-0.00756, "omega":-0.19658, "ax":0.0, "ay":-0.00021, "alpha":0.01726, "fx":[0.08537,0.07731,-0.08535,-0.07728], "fy":[-0.08079,0.08186,0.07379,-0.08886]},
+    {"t":3.1368, "x":3.75686, "y":0.68983, "heading":-3.10045, "vx":-1.99952, "vy":-0.00757, "omega":-0.19584, "ax":0.0, "ay":-0.00016, "alpha":0.02743, "fx":[0.13465,0.124,-0.13463,-0.12398], "fy":[-0.1267,0.13192,0.12128,-0.13735]},
+    {"t":3.17953, "x":3.67142, "y":0.68951, "heading":-3.10882, "vx":-1.99952, "vy":-0.00758, "omega":-0.19467, "ax":0.0, "ay":-0.00012, "alpha":0.03808, "fx":[0.18546,0.17369,-0.18544,-0.17367], "fy":[-0.1757,0.18343,0.17166,-0.18747]},
+    {"t":3.22226, "x":3.58598, "y":0.68918, "heading":-3.11714, "vx":-1.99952, "vy":-0.00758, "omega":-0.19304, "ax":0.0, "ay":-0.00008, "alpha":0.04939, "fx":[0.23865,0.22725,-0.23864,-0.22724], "fy":[-0.22864,0.23725,0.22585,-0.24004]},
+    {"t":3.26499, "x":3.50054, "y":0.68886, "heading":-3.12539, "vx":-1.99952, "vy":-0.00758, "omega":-0.19093, "ax":0.0, "ay":-0.00005, "alpha":0.06156, "fx":[0.29508,0.28567,-0.29507,-0.28565], "fy":[-0.28648,0.29425,0.28484,-0.29589]},
+    {"t":3.30772, "x":3.4151, "y":0.68853, "heading":-3.13355, "vx":-1.99952, "vy":-0.00759, "omega":-0.1883, "ax":0.01094, "ay":0.00003, "alpha":0.07479, "fx":[0.53659,0.53091,-0.17471,-0.16903], "fy":[-0.34955,0.35607,0.35039,-0.35524]},
+    {"t":3.35045, "x":3.32967, "y":0.68821, "heading":3.14159, "vx":-1.99905, "vy":-0.00759, "omega":-0.1851, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]

From 032e5309d1d714d3819e861c1487158552517466 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sat, 14 Mar 2026 23:37:05 -0700
Subject: [PATCH 49/54] debounce on being in climb tolerance

---
 src/main/java/frc/robot/Autos.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 938b7448..512e8beb 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -251,7 +251,7 @@ public Command climbScorePath(Path path, AutoRoutine routine) {
         swerve.stop().until(() -> climber.atFullExtension()),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
-            Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
+            Commands.waitUntil(new Trigger(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose())).debounce(0.2))
                 .andThen(
                     // Commands.print("hooray!")
                     setAutoClimbReqTrue())));
@@ -267,7 +267,7 @@ public Command climbNoScorePath(Path path, AutoRoutine routine) {
         swerve.stop().until(() -> climber.atFullExtension()),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
-            Commands.waitUntil(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
+            Commands.waitUntil(new Trigger(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose())).debounce(0.2))
                 .andThen(setAutoClimbReqTrue())));
   }
 

From 4b75bfb6c62c64641e3e75f5cf8c7d8fc329a0e6 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sun, 15 Mar 2026 10:31:54 -0700
Subject: [PATCH 50/54] fmt

---
 src/main/java/frc/robot/Autos.java | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java
index 512e8beb..e522c47d 100644
--- a/src/main/java/frc/robot/Autos.java
+++ b/src/main/java/frc/robot/Autos.java
@@ -251,7 +251,9 @@ public Command climbScorePath(Path path, AutoRoutine routine) {
         swerve.stop().until(() -> climber.atFullExtension()),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
-            Commands.waitUntil(new Trigger(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose())).debounce(0.2))
+            Commands.waitUntil(
+                    new Trigger(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
+                        .debounce(0.2))
                 .andThen(
                     // Commands.print("hooray!")
                     setAutoClimbReqTrue())));
@@ -267,7 +269,9 @@ public Command climbNoScorePath(Path path, AutoRoutine routine) {
         swerve.stop().until(() -> climber.atFullExtension()),
         Commands.parallel(
             swerve.alignToClimb(() -> getClimbAutoTarget()),
-            Commands.waitUntil(new Trigger(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose())).debounce(0.2))
+            Commands.waitUntil(
+                    new Trigger(() -> swerve.isInAutoAimTolerance(getClimbAutoTarget().getPose()))
+                        .debounce(0.2))
                 .andThen(setAutoClimbReqTrue())));
   }
 

From 859e0984d5155ade3f83bb57311e9f34274af453 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sun, 15 Mar 2026 10:44:51 -0700
Subject: [PATCH 51/54] Update at 'Sun Mar 15 10:44:50 PDT 2026'

---
 src/main/java/frc/robot/utils/autoaim/AutoAim.java | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 5a70e116..4a78ae7d 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -121,6 +121,14 @@ public class AutoAim {
             // - 6
             ,
             1.35));
+                COMP_HUB_SHOT_TREE.put(
+        Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12 + 6),
+        new ShotData(
+            Rotation2d.fromDegrees(39),
+            40.5 - 6 + 3 + 2
+            // - 6
+            ,
+            1.35));
   }
 
   // Ig we'll see if we need more than 1 feed shot tree

From a18376e3542a423f22554452611bb774fb540d1f Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sun, 15 Mar 2026 13:45:09 -0700
Subject: [PATCH 52/54] Update at 'Sun Mar 15 13:45:08 PDT 2026'

---
 src/main/java/frc/robot/Superstructure.java   | 103 +++++++++++-------
 .../subsystems/shooter/TurretSubsystem.java   |  59 +++++-----
 .../java/frc/robot/utils/autoaim/AutoAim.java |   2 +-
 3 files changed, 94 insertions(+), 70 deletions(-)

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
index 98585d0f..f9fdb6b4 100644
--- a/src/main/java/frc/robot/Superstructure.java
+++ b/src/main/java/frc/robot/Superstructure.java
@@ -46,7 +46,8 @@ public enum SuperState {
     CLIMB,
     POST_CLIMB,
     SPIN_UP_SCORE_PRE_CLIMB,
-    SCORE_PRE_CLIMB;
+    SCORE_PRE_CLIMB,
+    DEFENSE;
     public final Trigger trigger;
 
     private SuperState() {
@@ -178,8 +179,14 @@ public boolean isScoringActive() {
   @AutoLogOutput(key = "Superstructure/Operator Pose Override?")
   private static boolean poseOverride = false;
 
-  @AutoLogOutput(key = "Superstructure/Fixed Shot")
-  private static FixedShotTarget fixedShotTarget = FixedShotTarget.NONE;
+  @AutoLogOutput(key = "Superstructure/Defense?")
+  private boolean defense = false;
+
+  @AutoLogOutput(key = "Superstructure/Defense Req")
+  private Trigger defenseReq = new Trigger(() -> defense);
+
+  // @AutoLogOutput(key = "Superstructure/Fixed Shot")
+  // private static FixedShotTarget fixedShotTarget = FixedShotTarget.NONE;
 
   /** Creates a new Superstructure. */
   public Superstructure(
@@ -210,6 +217,9 @@ private void addTriggers() {
     operator.x().onTrue(Commands.runOnce(() -> shotTarget = ShotTarget.SCORE));
     operator.y().onTrue(Commands.runOnce(() -> shotTarget = ShotTarget.FEED));
 
+    operator.povUp().onTrue(Commands.runOnce(() -> defense = true));
+    operator.povDown().onTrue(Commands.runOnce(() -> defense = false));
+
     // toggle for flow state
     operator
         .a()
@@ -228,38 +238,40 @@ private void addTriggers() {
     // operator.leftTrigger().onTrue(Commands.runOnce(() -> poseOverride = true));
     // operator.rightTrigger().onTrue(Commands.runOnce(() -> poseOverride = false));
 
-    operator
-        .povLeft()
-        .onTrue(
-            Commands.runOnce(
-                () -> {
-                  fixedShotTarget = FixedShotTarget.LEFT;
-                  poseOverride = true;
-                }));
-    operator
-        .povUp()
-        .onTrue(
-            Commands.runOnce(
-                () -> {
-                  fixedShotTarget = FixedShotTarget.MID;
-                  poseOverride = true;
-                }));
-    operator
-        .povRight()
-        .onTrue(
-            Commands.runOnce(
-                () -> {
-                  fixedShotTarget = FixedShotTarget.RIGHT;
-                  poseOverride = true;
-                }));
-    operator
-        .povDown()
-        .onTrue(
-            Commands.runOnce(
-                () -> {
-                  fixedShotTarget = FixedShotTarget.NONE;
-                }));
-
+    // operator
+    //     .povLeft()
+    //     .onTrue(
+    //         Commands.runOnce(
+    //             () -> {
+    //               fixedShotTarget = FixedShotTarget.LEFT;
+    //               poseOverride = true;
+    //             }));
+    // operator
+    //     .povUp()
+    //     .onTrue(
+    //         Commands.runOnce(
+    //             () -> {
+    //               fixedShotTarget = FixedShotTarget.MID;
+    //               poseOverride = true;
+    //             }));
+    // operator
+    //     .povRight()
+    //     .onTrue(
+    //         Commands.runOnce(
+    //             () -> {
+    //               fixedShotTarget = FixedShotTarget.RIGHT;
+    //               poseOverride = true;
+    //             }));
+    // operator
+    //     .povDown()
+    //     .onTrue(
+    //         Commands.runOnce(
+    //             () -> {
+    //               fixedShotTarget = FixedShotTarget.NONE;
+    //             }));
+
+    operator.povUp().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
+    operator.povDown().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
     shootReq =
         driver
             .rightTrigger()
@@ -369,6 +381,10 @@ private void addTransitions() {
 
     bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate());
 
+    defenseReq.onTrue(changeStateTo(SuperState.DEFENSE));
+
+    bindTransition(SuperState.DEFENSE, SuperState.IDLE, defenseReq.negate());
+
     (preClimbReq.and(climbReq.negate())
         // .and(() -> DriverStation.isTeleop())
         )
@@ -672,6 +688,13 @@ private void addCommands() {
                         : AutoAim.COMP_HUB_SHOT_TREE),
             swerve::getVelocityFieldRelative),
         climber.extend());
+
+    bindCommands(
+        SuperState.DEFENSE,
+        intake.restRetracted(),
+        indexer.rest(),
+        shooter.stopTurret(),
+        climber.retract());
   }
 
   public void periodic() {
@@ -844,7 +867,9 @@ public boolean canScore() {
     // (isOurShift() || !DriverStation.isFMSAttached())
     //     &&
     (inScoringArea() || poseOverride)
-        && (!swerve.isNearTrench() || poseOverride || fixedShotTarget != FixedShotTarget.NONE);
+        && (!swerve.isNearTrench() || poseOverride
+        // || fixedShotTarget != FixedShotTarget.NONE
+        );
   }
 
   public boolean canShoot() {
@@ -859,9 +884,9 @@ public static FeedTarget getFeedTarget() {
     return feedTarget;
   }
 
-  public static FixedShotTarget getFixedShotTarget() {
-    return fixedShotTarget;
-  }
+  // public static FixedShotTarget getFixedShotTarget() {
+  //   return fixedShotTarget;
+  // }
 
   public static boolean getPoseOverride() {
     return poseOverride;
diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
index 72ad01a1..b873e0e6 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
@@ -23,7 +23,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Superstructure;
 import frc.robot.components.cancoder.CANcoderIO;
 import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged;
 import frc.robot.utils.FieldUtils;
@@ -265,35 +264,35 @@ public Command score(
         .andThen(
             this.run(
                 () -> {
-                  switch (Superstructure.getFixedShotTarget()) {
-                      // in front of left trench with intake facing trench
-                    case LEFT:
-                      hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle());
-                      flywheelIO.setMotionProfiledFlywheelVelocity(
-                          AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec());
-                      turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE);
-                      // in front of tower with intake facing left (to avoid deadzone)
-                    case MID:
-                      hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle());
-                      flywheelIO.setMotionProfiledFlywheelVelocity(
-                          AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec());
-                      turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE);
-                      // in front of right trench with intake facing alliance wall
-                    case RIGHT:
-                      hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle());
-                      flywheelIO.setMotionProfiledFlywheelVelocity(
-                          AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec());
-                      turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE);
-                    case NONE:
-                      hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle());
-                      flywheelIO.setMotionProfiledFlywheelVelocity(
-                          shotDataSupplier.get().flywheelVelocityRotPerSec());
-                      turretIO.setTurretPosition(
-                          AutoAim.getTurretHubTargetRotation(
-                              FieldUtils.getCurrentHubTranslation(),
-                              robotPoseSupplier.get(),
-                              chassisSpeedsSupplier.get()));
-                  }
+                  // switch (Superstructure.getFixedShotTarget()) {
+                  //     // in front of left trench with intake facing trench
+                  //   case LEFT:
+                  //     hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle());
+                  //     flywheelIO.setMotionProfiledFlywheelVelocity(
+                  //         AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec());
+                  //     turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE);
+                  //     // in front of tower with intake facing left (to avoid deadzone)
+                  //   case MID:
+                  //     hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle());
+                  //     flywheelIO.setMotionProfiledFlywheelVelocity(
+                  //         AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec());
+                  //     turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE);
+                  //     // in front of right trench with intake facing alliance wall
+                  //   case RIGHT:
+                  //     hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle());
+                  //     flywheelIO.setMotionProfiledFlywheelVelocity(
+                  //         AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec());
+                  //     turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE);
+                  //   case NONE:
+                  hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle());
+                  flywheelIO.setMotionProfiledFlywheelVelocity(
+                      shotDataSupplier.get().flywheelVelocityRotPerSec());
+                  turretIO.setTurretPosition(
+                      AutoAim.getTurretHubTargetRotation(
+                          FieldUtils.getCurrentHubTranslation(),
+                          robotPoseSupplier.get(),
+                          chassisSpeedsSupplier.get()));
+                  // }
                 }));
   }
 
diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
index 4a78ae7d..e461d583 100644
--- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java
+++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java
@@ -121,7 +121,7 @@ public class AutoAim {
             // - 6
             ,
             1.35));
-                COMP_HUB_SHOT_TREE.put(
+    COMP_HUB_SHOT_TREE.put(
         Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12 + 6),
         new ShotData(
             Rotation2d.fromDegrees(39),

From d8721fe469c7a143088e4c0e74600d700eb9ef96 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sun, 15 Mar 2026 13:53:13 -0700
Subject: [PATCH 53/54] Update at 'Sun Mar 15 13:53:12 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index 98a39b1e..4016866b 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -196,7 +196,7 @@ public static TalonFXConfiguration getPivotConfig() {
     config.Feedback.SensorToMechanismRatio = 1;
 
     config.Slot0.kS = 0.05;
-    config.Slot0.kV = 8.0; // Might suck
+    config.Slot0.kV = 15.0;
     config.Slot0.kA = 0.0;
     config.Slot0.kG = 0.55;
     config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;

From 0ee970876dadc0f6e02be020b9dd0ee6128e12e4 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Sun, 15 Mar 2026 13:53:23 -0700
Subject: [PATCH 54/54] Update at 'Sun Mar 15 13:53:23 PDT 2026'

---
 .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
index 4016866b..25f8dc1e 100644
--- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java
@@ -196,12 +196,12 @@ public static TalonFXConfiguration getPivotConfig() {
     config.Feedback.SensorToMechanismRatio = 1;
 
     config.Slot0.kS = 0.05;
-    config.Slot0.kV = 15.0;
+    config.Slot0.kV = 8.0; // Might suck\
     config.Slot0.kA = 0.0;
     config.Slot0.kG = 0.55;
     config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
     config.Slot0.GravityArmPositionOffset = 0.0; // Maybe need this??
-    config.Slot0.kP = 8.0;
+    config.Slot0.kP = 15.0;
     config.Slot0.kD = 0.3;
 
     config.CurrentLimits.StatorCurrentLimit = 45.0; // glup